engine/include/support/quaternion.h

   1 // Copyright (C) 2002-2007 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 namespace irr
  14 {
  15 namespace core
  16 {
  17
  18 //! Quaternion class.
  19 class quaternion
  20 {
  21         public:
  22
  23                 //! Default Constructor
  24                 quaternion() : X(0.0f), Y(0.0f), Z(0.0f), W(1.0f) {}
  25
  26                 //! Constructor
  27                 quaternion(f32 x, f32 y, f32 z, f32 w) : X(x), Y(y), Z(z), W(w) { }
  28
  29                 //! Constructor which converts euler angles (radians) to a quaternion
  30                 quaternion(f32 x, f32 y, f32 z);
  31
  32                 //! Constructor which converts euler angles (radians) to a quaternion
  33                 quaternion(const vector3df& vec);
  34
  35                 //! Constructor which converts a matrix to a quaternion
  36                 quaternion(const matrix4& mat);
  37
  38                 //! equal operator
  39                 bool operator==(const quaternion& other) const;
  40
  41                 //! assignment operator
  42                 inline quaternion& operator=(const quaternion& other);
  43
  44                 //! matrix assignment operator
  45                 inline quaternion& operator=(const matrix4& other);
  46
  47                 //! add operator
  48                 quaternion operator+(const quaternion& other) const;
  49
  50                 //! multiplication operator
  51                 quaternion operator*(const quaternion& other) const;
  52
  53                 //! multiplication operator
  54                 quaternion operator*(f32 s) const;
  55
  56                 //! multiplication operator
  57                 quaternion& operator*=(f32 s);
  58
  59                 //! multiplication operator
  60                 vector3df operator* (const vector3df& v) const;
  61
  62                 //! multiplication operator
  63                 quaternion& operator*=(const quaternion& other);
  64
  65                 //! calculates the dot product
  66                 inline f32 getDotProduct(const quaternion& other) const;
  67
  68                 //! sets new quaternion
  69                 inline void set(f32 x, f32 y, f32 z, f32 w);
  70
  71                 //! sets new quaternion based on euler angles (radians)
  72                 inline void set(f32 x, f32 y, f32 z);
  73
  74                 //! sets new quaternion based on euler angles (radians)
  75                 inline void set(const core::vector3df& vec);
  76
  77                 //! normalizes the quaternion
  78                 inline quaternion& normalize();
  79
  80                 //! Creates a matrix from this quaternion
  81                 matrix4 getMatrix() const;
  82
  83                 //! Creates a matrix from this quaternion
  84                 void getMatrix( matrix4 &dest ) const;
  85
  86                 //! Creates a matrix from this quaternion
  87                 void getMatrix_transposed( matrix4 &dest ) const;
  88
  89                 //! Inverts this quaternion
  90                 void makeInverse();
  91
  92                 //! set this quaternion to the result of the interpolation between two quaternions
  93                 void slerp( quaternion q1, quaternion q2, f32 interpolate );
  94
  95                 //! axis must be unit length
  96                 //! The quaternion representing the rotation is
  97                 //!  q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
  98                 void fromAngleAxis (f32 angle, const vector3df& axis);
  99
 100                 //! Fills an angle (radians) around an axis (unit vector)
 101                 void toAngleAxis (f32 &angle, vector3df& axis) const;
 102
 103                 //! Output this quaternion to an euler angle (radians)
 104                 void toEuler(vector3df& euler) const;
 105
 106                 //! set quaternion to identity
 107                 void makeIdentity();
 108
 109                 //! sets quaternion to represent a rotation from one angle to another
 110                 void rotationFromTo(const vector3df& from, const vector3df& to);
 111
 112                 f32 X, Y, Z, W;
 113 };
 114
 115
 116 //! Constructor which converts euler angles to a quaternion
 117 inline quaternion::quaternion(f32 x, f32 y, f32 z)
 118 {
 119         set(x,y,z);
 120 }
 121
 122
 123 //! Constructor which converts euler angles to a quaternion
 124 inline quaternion::quaternion(const vector3df& vec)
 125 {
 126         set(vec.X,vec.Y,vec.Z);
 127 }
 128
 129
 130 //! Constructor which converts a matrix to a quaternion
 131 inline quaternion::quaternion(const matrix4& mat)
 132 {
 133         (*this) = mat;
 134 }
 135
 136
 137 //! equal operator
 138 inline bool quaternion::operator==(const quaternion& other) const
 139 {
 140         if(X != other.X)
 141                 return false;
 142         if(Y != other.Y)
 143                 return false;
 144         if(Z != other.Z)
 145                 return false;
 146         if(W != other.W)
 147                 return false;
 148
 149         return true;
 150 }
 151
 152
 153 //! assignment operator
 154 inline quaternion& quaternion::operator=(const quaternion& other)
 155 {
 156         X = other.X;
 157         Y = other.Y;
 158         Z = other.Z;
 159         W = other.W;
 160         return *this;
 161 }
 162
 163
 164 //! matrix assignment operator
 165 inline quaternion& quaternion::operator=(const matrix4& m)
 166 {
 167         f32 diag = m(0,0) + m(1,1) + m(2,2) + 1;
 168         f32 scale = 0.0f;
 169
 170         if( diag > 0.0f )
 171         {
 172                 scale = sqrtf(diag) * 2.0f; // get scale from diagonal
 173
 174                 // TODO: speed this up
 175                 X = ( m(2,1) - m(1,2)) / scale;
 176                 Y = ( m(0,2) - m(2,0)) / scale;
 177                 Z = ( m(1,0) - m(0,1)) / scale;
 178                 W = 0.25f * scale;
 179         }
 180         else
 181         {
 182                 if ( m(0,0) > m(1,1) && m(0,0) > m(2,2))
 183                 {
 184                         // 1st element of diag is greatest value
 185                         // find scale according to 1st element, and double it
 186                         scale = sqrtf( 1.0f + m(0,0) - m(1,1) - m(2,2)) * 2.0f;
 187
 188                         // TODO: speed this up
 189                         X = 0.25f * scale;
 190                         Y = (m(0,1) + m(1,0)) / scale;
 191                         Z = (m(2,0) + m(0,2)) / scale;
 192                         W = (m(2,1) - m(1,2)) / scale;
 193                 }
 194                 else if ( m(1,1) > m(2,2))
 195                 {
 196                         // 2nd element of diag is greatest value
 197                         // find scale according to 2nd element, and double it
 198                         scale = sqrtf( 1.0f + m(1,1) - m(0,0) - m(2,2)) * 2.0f;
 199
 200                         // TODO: speed this up
 201                         X = (m(0,1) + m(1,0) ) / scale;
 202                         Y = 0.25f * scale;
 203                         Z = (m(1,2) + m(2,1) ) / scale;
 204                         W = (m(0,2) - m(2,0) ) / scale;
 205                 }
 206                 else
 207                 {
 208                         // 3rd element of diag is greatest value
 209                         // find scale according to 3rd element, and double it
 210                         scale  = sqrtf( 1.0f + m(2,2) - m(0,0) - m(1,1)) * 2.0f;
 211
 212                         // TODO: speed this up
 213                         X = (m(0,2) + m(2,0)) / scale;
 214                         Y = (m(1,2) + m(2,1)) / scale;
 215                         Z = 0.25f * scale;
 216                         W = (m(1,0) - m(0,1)) / scale;
 217                 }
 218         }
 219
 220         normalize();
 221         return *this;
 222 }
 223
 224
 225 //! multiplication operator
 226 inline quaternion quaternion::operator*(const quaternion& other) const
 227 {
 228         quaternion tmp;
 229
 230         tmp.W = (other.W * W) - (other.X * X) - (other.Y * Y) - (other.Z * Z);
 231         tmp.X = (other.W * X) + (other.X * W) + (other.Y * Z) - (other.Z * Y);
 232         tmp.Y = (other.W * Y) + (other.Y * W) + (other.Z * X) - (other.X * Z);
 233         tmp.Z = (other.W * Z) + (other.Z * W) + (other.X * Y) - (other.Y * X);
 234
 235         return tmp;
 236 }
 237
 238
 239 //! multiplication operator
 240 inline quaternion quaternion::operator*(f32 s) const
 241 {
 242         return quaternion(s*X, s*Y, s*Z, s*W);
 243 }
 244
 245 //! multiplication operator
 246 inline quaternion& quaternion::operator*=(f32 s)
 247 {
 248         X *= s; Y*=s; Z*=s; W*=s;
 249         return *this;
 250 }
 251
 252 //! multiplication operator
 253 inline quaternion& quaternion::operator*=(const quaternion& other)
 254 {
 255         *this = other * (*this);
 256         return *this;
 257 }
 258
 259 //! add operator
 260 inline quaternion quaternion::operator+(const quaternion& b) const
 261 {
 262         return quaternion(X+b.X, Y+b.Y, Z+b.Z, W+b.W);
 263 }
 264
 265
 266 //! Creates a matrix from this quaternion
 267 inline matrix4 quaternion::getMatrix() const
 268 {
 269         core::matrix4 m;
 270
 271         m(0,0) = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
 272         m(1,0) = 2.0f*X*Y + 2.0f*Z*W;
 273         m(2,0) = 2.0f*X*Z - 2.0f*Y*W;
 274         m(3,0) = 0.0f;
 275
 276         m(0,1) = 2.0f*X*Y - 2.0f*Z*W;
 277         m(1,1) = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
 278         m(2,1) = 2.0f*Z*Y + 2.0f*X*W;
 279         m(3,1) = 0.0f;
 280
 281         m(0,2) = 2.0f*X*Z + 2.0f*Y*W;
 282         m(1,2) = 2.0f*Z*Y - 2.0f*X*W;
 283         m(2,2) = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
 284         m(3,2) = 0.0f;
 285
 286         m(0,3) = 0.0f;
 287         m(1,3) = 0.0f;
 288         m(2,3) = 0.0f;
 289         m(3,3) = 1.0f;
 290
 291         return m;
 292 }
 293
 294
 295 //! Creates a matrix from this quaternion
 296 inline void quaternion::getMatrix( matrix4 &dest ) const
 297 {
 298         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
 299         dest[1] = 2.0f*X*Y + 2.0f*Z*W;
 300         dest[2] = 2.0f*X*Z - 2.0f*Y*W;
 301         dest[3] = 0.0f;
 302
 303         dest[4] = 2.0f*X*Y - 2.0f*Z*W;
 304         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
 305         dest[6] = 2.0f*Z*Y + 2.0f*X*W;
 306         dest[7] = 0.0f;
 307
 308         dest[8] = 2.0f*X*Z + 2.0f*Y*W;
 309         dest[9] = 2.0f*Z*Y - 2.0f*X*W;
 310         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
 311         dest[11] = 0.0f;
 312
 313         dest[12] = 0.f;
 314         dest[13] = 0.f;
 315         dest[14] = 0.f;
 316         dest[15] = 1.f;
 317 }
 318
 319 //! Creates a matrix from this quaternion
 320 inline void quaternion::getMatrix_transposed( matrix4 &dest ) const
 321 {
 322         dest[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
 323         dest[4] = 2.0f*X*Y + 2.0f*Z*W;
 324         dest[8] = 2.0f*X*Z - 2.0f*Y*W;
 325         dest[12] = 0.0f;
 326
 327         dest[1] = 2.0f*X*Y - 2.0f*Z*W;
 328         dest[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
 329         dest[9] = 2.0f*Z*Y + 2.0f*X*W;
 330         dest[13] = 0.0f;
 331
 332         dest[2] = 2.0f*X*Z + 2.0f*Y*W;
 333         dest[6] = 2.0f*Z*Y - 2.0f*X*W;
 334         dest[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
 335         dest[14] = 0.0f;
 336
 337         dest[3] = 0.f;
 338         dest[7] = 0.f;
 339         dest[11] = 0.f;
 340         dest[15] = 1.f;
 341 }
 342
 343
 344
 345 //! Inverts this quaternion
 346 inline void quaternion::makeInverse()
 347 {
 348         X = -X; Y = -Y; Z = -Z;
 349 }
 350
 351 //! sets new quaternion
 352 inline void quaternion::set(f32 x, f32 y, f32 z, f32 w)
 353 {
 354         X = x;
 355         Y = y;
 356         Z = z;
 357         W = w;
 358 }
 359
 360
 361 //! sets new quaternion based on euler angles
 362 inline void quaternion::set(f32 x, f32 y, f32 z)
 363 {
 364         f64 angle;
 365
 366         angle = x * 0.5;
 367         f64 sr = (f32)sin(angle);
 368         f64 cr = (f32)cos(angle);
 369
 370         angle = y * 0.5;
 371         f64 sp = (f32)sin(angle);
 372         f64 cp = (f32)cos(angle);
 373
 374         angle = z * 0.5;
 375         f64 sy = (f32)sin(angle);
 376         f64 cy = (f32)cos(angle);
 377
 378         f64 cpcy = cp * cy;
 379         f64 spcy = sp * cy;
 380         f64 cpsy = cp * sy;
 381         f64 spsy = sp * sy;
 382
 383         X = (f32)(sr * cpcy - cr * spsy);
 384         Y = (f32)(cr * spcy + sr * cpsy);
 385         Z = (f32)(cr * cpsy - sr * spcy);
 386         W = (f32)(cr * cpcy + sr * spsy);
 387
 388         normalize();
 389 }
 390
 391 //! sets new quaternion based on euler angles
 392 inline void quaternion::set(const core::vector3df& vec)
 393 {
 394         set(vec.X, vec.Y, vec.Z);
 395 }
 396
 397 //! normalizes the quaternion
 398 inline quaternion& quaternion::normalize()
 399 {
 400         f32 n = X*X + Y*Y + Z*Z + W*W;
 401
 402         if (n == 1)
 403                 return *this;
 404
 405         //n = 1.0f / sqrtf(n);
 406         n = reciprocal_squareroot ( n );
 407         X *= n;
 408         Y *= n;
 409         Z *= n;
 410         W *= n;
 411
 412         return *this;
 413 }
 414
 415
 416 // set this quaternion to the result of the interpolation between two quaternions
 417 inline void quaternion::slerp( quaternion q1, quaternion q2, f32 time)
 418 {
 419         f32 angle = q1.getDotProduct(q2);
 420
 421         if (angle < 0.0f)
 422         {
 423                 q1 *= -1.0f;
 424                 angle *= -1.0f;
 425         }
 426
 427         f32 scale;
 428         f32 invscale;
 429
 430         if ((angle + 1.0f) > 0.05f)
 431         {
 432                 if ((1.0f - angle) >= 0.05f)  // spherical interpolation
 433                 {
 434                         f32 theta = (f32)acos(angle);
 435                         f32 invsintheta = 1.0f / (f32)sin(theta);
 436                         scale = (f32)sin(theta * (1.0f-time)) * invsintheta;
 437                         invscale = (f32)sin(theta * time) * invsintheta;
 438                 }
 439                 else // linear interploation
 440                 {
 441                         scale = 1.0f - time;
 442                         invscale = time;
 443                 }
 444         }
 445         else
 446         {
 447                 q2.set(-q1.Y, q1.X, -q1.W, q1.Z);
 448                 scale = (f32)sin(PI * (0.5f - time));
 449                 invscale = (f32)sin(PI * time);
 450         }
 451
 452         *this = (q1*scale) + (q2*invscale);
 453 }
 454
 455
 456 //! calculates the dot product
 457 inline f32 quaternion::getDotProduct(const quaternion& q2) const
 458 {
 459         return (X * q2.X) + (Y * q2.Y) + (Z * q2.Z) + (W * q2.W);
 460 }
 461
 462
 463 inline void quaternion::fromAngleAxis(f32 angle, const vector3df& axis)
 464 {
 465         f32 fHalfAngle = 0.5f*angle;
 466         f32 fSin = (f32)sin(fHalfAngle);
 467         W = (f32)cos(fHalfAngle);
 468         X = fSin*axis.X;
 469         Y = fSin*axis.Y;
 470         Z = fSin*axis.Z;
 471 }
 472
 473 inline void quaternion::toAngleAxis(f32 &angle, core::vector3df &axis) const
 474 {
 475         f32 scale = sqrtf(X*X + Y*Y + Z*Z);
 476
 477         if (core::iszero(scale) || W > 1.0f || W < -1.0f)
 478         {
 479                 angle = 0.0f;
 480                 axis.X = 0.0f;
 481                 axis.Y = 1.0f;
 482                 axis.Z = 0.0f;
 483         }
 484         else
 485         {
 486                 angle = 2.0f * acos(W);
 487                 axis.X = X / scale;
 488                 axis.Y = Y / scale;
 489                 axis.Z = Z / scale;
 490         }
 491 }
 492
 493 inline void quaternion::toEuler(vector3df& euler) const
 494 {
 495         double sqw = W*W;
 496         double sqx = X*X;
 497         double sqy = Y*Y;
 498         double sqz = Z*Z;
 499
 500         // heading = rotation about z-axis
 501         euler.Z = (f32) (atan2(2.0 * (X*Y +Z*W),(sqx - sqy - sqz + sqw)));
 502
 503         // bank = rotation about x-axis
 504         euler.X = (f32) (atan2(2.0 * (Y*Z +X*W),(-sqx - sqy + sqz + sqw)));
 505
 506         // attitude = rotation about y-axis
 507         euler.Y = (f32) (asin( clamp(-2.0 * (X*Z - Y*W), -1.0, 1.0) ));
 508 }
 509
 510 inline vector3df quaternion::operator* (const vector3df& v) const
 511 {
 512         // nVidia SDK implementation
 513
 514         vector3df uv, uuv;
 515         vector3df qvec(X, Y, Z);
 516         uv = qvec.crossProduct(v);
 517         uuv = qvec.crossProduct(uv);
 518         uv *= (2.0f * W);
 519         uuv *= 2.0f;
 520
 521         return v + uv + uuv;
 522 }
 523
 524 //! set quaterion to identity
 525 inline void quaternion::makeIdentity()
 526 {
 527         W = 1.f;
 528         X = 0.f;
 529         Y = 0.f;
 530         Z = 0.f;
 531 }
 532
 533 inline void quaternion::rotationFromTo(const vector3df& from, const vector3df& to)
 534 {
 535         // Based on Stan Melax's article in Game Programming Gems
 536         // Copy, since cannot modify local
 537         vector3df v0 = from;
 538         vector3df v1 = to;
 539         v0.normalize();
 540         v1.normalize();
 541
 542         vector3df c = v0.crossProduct(v1);
 543
 544         f32 d = v0.dotProduct(v1);
 545         if (d >= 1.0f) // If dot == 1, vectors are the same
 546         {
 547                 *this=quaternion(0,0,0,1); //IDENTITY;
 548         }
 549         f32 s = sqrtf( (1+d)*2 ); // optimize inv_sqrt
 550         f32 invs = 1 / s;
 551
 552         X = c.X * invs;
 553         Y = c.Y * invs;
 554         Z = c.Z * invs;
 555         W = s * 0.5f;
 556 }
 557
 558
 559 } // end namespace core
 560 } // end namespace irr
 561
 562 #endif
 563