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 bool operator!=(const quaternion& other) const;
00045
00047 inline quaternion& operator=(const quaternion& other);
00048
00050 inline quaternion& operator=(const matrix4& other);
00051
00053 quaternion operator+(const quaternion& other) const;
00054
00056 quaternion operator*(const quaternion& other) const;
00057
00059 quaternion operator*(f32 s) const;
00060
00062 quaternion& operator*=(f32 s);
00063
00065 vector3df operator*(const vector3df& v) const;
00066
00068 quaternion& operator*=(const quaternion& other);
00069
00071 inline f32 dotProduct(const quaternion& other) const;
00072
00074 inline quaternion& set(f32 x, f32 y, f32 z, f32 w);
00075
00077 inline quaternion& set(f32 x, f32 y, f32 z);
00078
00080 inline quaternion& set(const core::vector3df& vec);
00081
00083 inline quaternion& set(const core::quaternion& quat);
00084
00086 inline bool equals(const quaternion& other,
00087 const f32 tolerance = ROUNDING_ERROR_f32 ) const;
00088
00090 inline quaternion& normalize();
00091
00093 matrix4 getMatrix() const;
00094
00096 void getMatrix( matrix4 &dest, const core::vector3df &translation ) const;
00097
00115 void getMatrixCenter( matrix4 &dest, const core::vector3df ¢er, const core::vector3df &translation ) const;
00116
00118 inline void getMatrix_transposed( matrix4 &dest ) const;
00119
00121 quaternion& makeInverse();
00122
00124 quaternion& slerp( quaternion q1, quaternion q2, f32 interpolate );
00125
00127
00132 quaternion& fromAngleAxis (f32 angle, const vector3df& axis);
00133
00135 void toAngleAxis (f32 &angle, core::vector3df& axis) const;
00136
00138 void toEuler(vector3df& euler) const;
00139
00141 quaternion& makeIdentity();
00142
00144 quaternion& rotationFromTo(const vector3df& from, const vector3df& to);
00145
00147 f32 X;
00148 f32 Y;
00149 f32 Z;
00150 f32 W;
00151 };
00152
00153
00154
00155 inline quaternion::quaternion(f32 x, f32 y, f32 z)
00156 {
00157 set(x,y,z);
00158 }
00159
00160
00161
00162 inline quaternion::quaternion(const vector3df& vec)
00163 {
00164 set(vec.X,vec.Y,vec.Z);
00165 }
00166
00167
00168
00169 inline quaternion::quaternion(const matrix4& mat)
00170 {
00171 (*this) = mat;
00172 }
00173
00174
00175
00176 inline bool quaternion::operator==(const quaternion& other) const
00177 {
00178 return ((X == other.X) &&
00179 (Y == other.Y) &&
00180 (Z == other.Z) &&
00181 (W == other.W));
00182 }
00183
00184
00185 inline bool quaternion::operator!=(const quaternion& other) const
00186 {
00187 return !(*this == other);
00188 }
00189
00190
00191 inline quaternion& quaternion::operator=(const quaternion& other)
00192 {
00193 X = other.X;
00194 Y = other.Y;
00195 Z = other.Z;
00196 W = other.W;
00197 return *this;
00198 }
00199
00200
00201
00202 inline quaternion& quaternion::operator=(const matrix4& m)
00203 {
00204 const f32 diag = m(0,0) + m(1,1) + m(2,2) + 1;
00205
00206 if( diag > 0.0f )
00207 {
00208 const f32 scale = sqrtf(diag) * 2.0f;
00209
00210
00211 X = ( m(2,1) - m(1,2)) / scale;
00212 Y = ( m(0,2) - m(2,0)) / scale;
00213 Z = ( m(1,0) - m(0,1)) / scale;
00214 W = 0.25f * scale;
00215 }
00216 else
00217 {
00218 if ( m(0,0) > m(1,1) && m(0,0) > m(2,2))
00219 {
00220
00221
00222 const f32 scale = sqrtf( 1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;
00223
00224
00225 X = 0.25f * scale;
00226 Y = (m(0,1) + m(1,0)) / scale;
00227 Z = (m(2,0) + m(0,2)) / scale;
00228 W = (m(2,1) - m(1,2)) / scale;
00229 }
00230 else if ( m(1,1) > m(2,2))
00231 {
00232
00233
00234 const f32 scale = sqrtf( 1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;
00235
00236
00237 X = (m(0,1) + m(1,0) ) / scale;
00238 Y = 0.25f * scale;
00239 Z = (m(1,2) + m(2,1) ) / scale;
00240 W = (m(0,2) - m(2,0) ) / scale;
00241 }
00242 else
00243 {
00244
00245
00246 const f32 scale = sqrtf( 1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;
00247
00248
00249 X = (m(0,2) + m(2,0)) / scale;
00250 Y = (m(1,2) + m(2,1)) / scale;
00251 Z = 0.25f * scale;
00252 W = (m(1,0) - m(0,1)) / scale;
00253 }
00254 }
00255
00256 return normalize();
00257 }
00258
00259
00260
00261 inline quaternion quaternion::operator*(const quaternion& other) const
00262 {
00263 quaternion tmp;
00264
00265 tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
00266 tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
00267 tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
00268 tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
00269
00270 return tmp;
00271 }
00272
00273
00274
00275 inline quaternion quaternion::operator*(f32 s) const
00276 {
00277 return quaternion(s*X, s*Y, s*Z, s*W);
00278 }
00279
00280
00281 inline quaternion& quaternion::operator*=(f32 s)
00282 {
00283 X*=s;
00284 Y*=s;
00285 Z*=s;
00286 W*=s;
00287 return *this;
00288 }
00289
00290
00291 inline quaternion& quaternion::operator*=(const quaternion& other)
00292 {
00293 return (*this = other * (*this));
00294 }
00295
00296
00297 inline quaternion quaternion::operator+(const quaternion& b) const
00298 {
00299 return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
00300 }
00301
00302
00303
00304 inline matrix4 quaternion::getMatrix() const
00305 {
00306 core::matrix4 m;
00307 getMatrix_transposed(m);
00308 return m;
00309 }
00310
00311
00315 inline void quaternion::getMatrix( matrix4 &dest, const core::vector3df ¢er ) const
00316 {
00317 f32 * m = dest.pointer();
00318
00319 m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
00320 m[1] = 2.0f*X*Y + 2.0f*Z*W;
00321 m[2] = 2.0f*X*Z - 2.0f*Y*W;
00322 m[3] = 0.0f;
00323
00324 m[4] = 2.0f*X*Y - 2.0f*Z*W;
00325 m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
00326 m[6] = 2.0f*Z*Y + 2.0f*X*W;
00327 m[7] = 0.0f;
00328
00329 m[8] = 2.0f*X*Z + 2.0f*Y*W;
00330 m[9] = 2.0f*Z*Y - 2.0f*X*W;
00331 m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
00332 m[11] = 0.0f;
00333
00334 m[12] = center.X;
00335 m[13] = center.Y;
00336 m[14] = center.Z;
00337 m[15] = 1.f;
00338
00339
00340 dest.setDefinitelyIdentityMatrix ( false );
00341 }
00342
00343
00344
00357 inline void quaternion::getMatrixCenter(matrix4 &dest,
00358 const core::vector3df ¢er,
00359 const core::vector3df &translation) const
00360 {
00361 f32 * m = dest.pointer();
00362
00363 m[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
00364 m[1] = 2.0f*X*Y + 2.0f*Z*W;
00365 m[2] = 2.0f*X*Z - 2.0f*Y*W;
00366 m[3] = 0.0f;
00367
00368 m[4] = 2.0f*X*Y - 2.0f*Z*W;
00369 m[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
00370 m[6] = 2.0f*Z*Y + 2.0f*X*W;
00371 m[7] = 0.0f;
00372
00373 m[8] = 2.0f*X*Z + 2.0f*Y*W;
00374 m[9] = 2.0f*Z*Y - 2.0f*X*W;
00375 m[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
00376 m[11] = 0.0f;
00377
00378 dest.setRotationCenter ( center, translation );
00379 }
00380
00381
00382 inline void quaternion::getMatrix_transposed( matrix4 &dest ) const
00383 {
00384 dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
00385 dest[4] = 2.0f*X*Y + 2.0f*Z*W;
00386 dest[8] = 2.0f*X*Z - 2.0f*Y*W;
00387 dest[12] = 0.0f;
00388
00389 dest[1] = 2.0f*X*Y - 2.0f*Z*W;
00390 dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
00391 dest[9] = 2.0f*Z*Y + 2.0f*X*W;
00392 dest[13] = 0.0f;
00393
00394 dest[2] = 2.0f*X*Z + 2.0f*Y*W;
00395 dest[6] = 2.0f*Z*Y - 2.0f*X*W;
00396 dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
00397 dest[14] = 0.0f;
00398
00399 dest[3] = 0.f;
00400 dest[7] = 0.f;
00401 dest[11] = 0.f;
00402 dest[15] = 1.f;
00403
00404 dest.setDefinitelyIdentityMatrix ( false );
00405 }
00406
00407
00408
00409
00410 inline quaternion& quaternion::makeInverse()
00411 {
00412 X = -X; Y = -Y; Z = -Z;
00413 return *this;
00414 }
00415
00416
00417 inline quaternion& quaternion::set(f32 x, f32 y, f32 z, f32 w)
00418 {
00419 X = x;
00420 Y = y;
00421 Z = z;
00422 W = w;
00423 return *this;
00424 }
00425
00426
00427
00428 inline quaternion& quaternion::set(f32 x, f32 y, f32 z)
00429 {
00430 f64 angle;
00431
00432 angle = x * 0.5;
00433 const f64 sr = sin(angle);
00434 const f64 cr = cos(angle);
00435
00436 angle = y * 0.5;
00437 const f64 sp = sin(angle);
00438 const f64 cp = cos(angle);
00439
00440 angle = z * 0.5;
00441 const f64 sy = sin(angle);
00442 const f64 cy = cos(angle);
00443
00444 const f64 cpcy = cp * cy;
00445 const f64 spcy = sp * cy;
00446 const f64 cpsy = cp * sy;
00447 const f64 spsy = sp * sy;
00448
00449 X = (f32)(sr * cpcy - cr * spsy);
00450 Y = (f32)(cr * spcy + sr * cpsy);
00451 Z = (f32)(cr * cpsy - sr * spcy);
00452 W = (f32)(cr * cpcy + sr * spsy);
00453
00454 return normalize();
00455 }
00456
00457
00458 inline quaternion& quaternion::set(const core::vector3df& vec)
00459 {
00460 return set(vec.X, vec.Y, vec.Z);
00461 }
00462
00463
00464 inline quaternion& quaternion::set(const core::quaternion& quat)
00465 {
00466 return (*this=quat);
00467 }
00468
00469
00471 inline bool quaternion::equals(const quaternion& other, const f32 tolerance) const
00472 {
00473 return core::equals(X, other.X, tolerance) &&
00474 core::equals(Y, other.Y, tolerance) &&
00475 core::equals(Z, other.Z, tolerance) &&
00476 core::equals(W, other.W, tolerance);
00477 }
00478
00479
00480
00481 inline quaternion& quaternion::normalize()
00482 {
00483 const f32 n = X*X + Y*Y + Z*Z + W*W;
00484
00485 if (n == 1)
00486 return *this;
00487
00488
00489 return (*this *= reciprocal_squareroot ( n ));
00490 }
00491
00492
00493
00494 inline quaternion& quaternion::slerp(quaternion q1, quaternion q2, f32 time)
00495 {
00496 f32 angle = q1.dotProduct(q2);
00497
00498 if (angle < 0.0f)
00499 {
00500 q1 *= -1.0f;
00501 angle *= -1.0f;
00502 }
00503
00504 f32 scale;
00505 f32 invscale;
00506
00507 if ((angle + 1.0f) > 0.05f)
00508 {
00509 if ((1.0f - angle) >= 0.05f)
00510 {
00511 const f32 theta = acosf(angle);
00512 const f32 invsintheta = reciprocal(sinf(theta));
00513 scale = sinf(theta * (1.0f-time)) * invsintheta;
00514 invscale = sinf(theta * time) * invsintheta;
00515 }
00516 else
00517 {
00518 scale = 1.0f - time;
00519 invscale = time;
00520 }
00521 }
00522 else
00523 {
00524 q2.set(-q1.Y, q1.X, -q1.W, q1.Z);
00525 scale = sinf(PI * (0.5f - time));
00526 invscale = sinf(PI * time);
00527 }
00528
00529 return (*this = (q1*scale) + (q2*invscale));
00530 }
00531
00532
00533
00534 inline f32 quaternion::dotProduct(const quaternion& q2) const
00535 {
00536 return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
00537 }
00538
00539
00542 inline quaternion& quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
00543 {
00544 const f32 fHalfAngle = 0.5f*angle;
00545 const f32 fSin = sinf(fHalfAngle);
00546 W = cosf(fHalfAngle);
00547 X = fSin*axis.X;
00548 Y = fSin*axis.Y;
00549 Z = fSin*axis.Z;
00550 return *this;
00551 }
00552
00553
00554 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
00555 {
00556 const f32 scale = sqrtf(X*X + Y*Y + Z*Z);
00557
00558 if (core::iszero(scale) || W > 1.0f || W < -1.0f)
00559 {
00560 angle = 0.0f;
00561 axis.X = 0.0f;
00562 axis.Y = 1.0f;
00563 axis.Z = 0.0f;
00564 }
00565 else
00566 {
00567 const f32 invscale = reciprocal(scale);
00568 angle = 2.0f * acosf(W);
00569 axis.X = X * invscale;
00570 axis.Y = Y * invscale;
00571 axis.Z = Z * invscale;
00572 }
00573 }
00574
00575 inline void quaternion::toEuler(vector3df& euler) const
00576 {
00577 const f64 sqw = W*W;
00578 const f64 sqx = X*X;
00579 const f64 sqy = Y*Y;
00580 const f64 sqz = Z*Z;
00581
00582
00583 euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
00584
00585
00586 euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
00587
00588
00589 euler.Y = asinf( clamp(-2.0f * (X*Z - Y*W), -1.0f, 1.0f) );
00590 }
00591
00592
00593 inline vector3df quaternion::operator* (const vector3df& v) const
00594 {
00595
00596
00597 vector3df uv, uuv;
00598 vector3df qvec(X, Y, Z);
00599 uv = qvec.crossProduct(v);
00600 uuv = qvec.crossProduct(uv);
00601 uv *= (2.0f * W);
00602 uuv *= 2.0f;
00603
00604 return v + uv + uuv;
00605 }
00606
00607
00608 inline core::quaternion& quaternion::makeIdentity()
00609 {
00610 W = 1.f;
00611 X = 0.f;
00612 Y = 0.f;
00613 Z = 0.f;
00614 return *this;
00615 }
00616
00617 inline core::quaternion& quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
00618 {
00619
00620
00621 vector3df v0 = from;
00622 vector3df v1 = to;
00623 v0.normalize();
00624 v1.normalize();
00625
00626 const f32 d = v0.dotProduct(v1);
00627 if (d >= 1.0f)
00628 {
00629 return makeIdentity();
00630 }
00631 else if (d <= -1.0f)
00632 {
00633 core::vector3df axis(1.0f, 0.f, 0.f);
00634 axis = axis.crossProduct(core::vector3df(X,Y,Z));
00635 if (axis.getLength()==0)
00636 {
00637 axis.set(0.f,1.f,0.f);
00638 axis.crossProduct(core::vector3df(X,Y,Z));
00639 }
00640 return this->fromAngleAxis(core::PI, axis);
00641 }
00642
00643 const f32 s = sqrtf( (1+d)*2 );
00644 const f32 invs = 1.f / s;
00645 const vector3df c = v0.crossProduct(v1)*invs;
00646 X = c.X;
00647 Y = c.Y;
00648 Z = c.Z;
00649 W = s * 0.5f;
00650
00651 return *this;
00652 }
00653
00654
00655 }
00656 }
00657
00658 #endif
00659