Source/PhysicsMath.h

   1 /*
   2 Copyright (C) 2003, 2010 - Wolfire Games
   3
   4 This file is part of Lugaru.
   5
   6 Lugaru is free software; you can redistribute it and/or
   7 modify it under the terms of the GNU General Public License
   8 as published by the Free Software Foundation; either version 2
   9 of the License, or (at your option) any later version.
  10
  11 This program is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  14
  15 See the GNU General Public License for more details.
  16
  17 You should have received a copy of the GNU General Public License
  18 along with this program; if not, write to the Free Software
  19 Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  20 */
  21
  22 #ifndef _PHYSICSMATH_H_
  23 #define _PHYSICSMATH_H_
  24
  25 //#include <Carbon.h>
  26
  27 #include "MacCompatibility.h"
  28
  29 //------------------------------------------------------------------------//
  30 // Misc. Constants
  31 //------------------------------------------------------------------------//
  32
  33 float   const   pi      = 3.14159265f;
  34 float   const   g       = -32.174f;             // acceleration due to gravity, ft/s^2
  35 float   const   rho = 0.0023769f;       // desity of air at sea level, slugs/ft^3
  36 float   const   tol = 0.0000000001f;            // float type tolerance
  37
  38
  39 //------------------------------------------------------------------------//
  40 // Misc. Functions
  41 //------------------------------------------------------------------------//
  42 inline  float   DegreesToRadians(float deg);
  43 inline  float   RadiansToDegrees(float rad);
  44
  45 inline  float   DegreesToRadians(float deg)
  46 {
  47     return deg * pi / 180.0f;
  48 }
  49
  50 inline  float   RadiansToDegrees(float rad)
  51 {
  52     return rad * 180.0f / pi;
  53 }
  54
  55 //------------------------------------------------------------------------//
  56 // Vector Class and vector functions
  57 //------------------------------------------------------------------------//
  58 class Vector
  59 {
  60 public:
  61     float x;
  62     float y;
  63     float z;
  64
  65     Vector(void);
  66     Vector(float xi, float yi, float zi);
  67
  68     float Magnitude(void);
  69     void  Normalize(void);
  70     void  Reverse(void);
  71
  72     Vector& operator+=(Vector u);       // vector addition
  73     Vector& operator-=(Vector u);       // vector subtraction
  74     Vector& operator*=(float s);        // scalar multiply
  75     Vector& operator/=(float s);        // scalar divide
  76
  77     Vector operator-(void);
  78
  79 };
  80
  81 inline  Vector operator+(Vector u, Vector v);
  82 inline  Vector operator-(Vector u, Vector v);
  83 inline  Vector operator^(Vector u, Vector v);
  84 inline  float operator*(Vector u, Vector v);
  85 inline  Vector operator*(float s, Vector u);
  86 inline  Vector operator*(Vector u, float s);
  87 inline  Vector operator/(Vector u, float s);
  88 inline  float TripleScalarProduct(Vector u, Vector v, Vector w);
  89 /*
  90 float fast_sqrt2 (register float arg);
  91 float fast_sqrt2 (register float arg)
  92 {
  93 // Can replace with slower return std::sqrt(arg);
  94 register float result;
  95
  96 if (arg == 0.0) return 0.0;
  97
  98 asm {
  99 frsqrte         result,arg                      // Calculate Square root
 100 }
 101
 102 // Newton Rhapson iterations.
 103 result = result + 0.5 * result * (1.0 - arg * result * result);
 104 result = result + 0.5 * result * (1.0 - arg * result * result);
 105
 106 return result * arg;
 107 }
 108 */
 109 inline Vector::Vector(void)
 110 {
 111     x = 0;
 112     y = 0;
 113     z = 0;
 114 }
 115
 116 inline Vector::Vector(float xi, float yi, float zi)
 117 {
 118     x = xi;
 119     y = yi;
 120     z = zi;
 121 }
 122
 123 inline  float Vector::Magnitude(void)
 124 {
 125     return (float) sqrt(x * x + y * y + z * z);
 126 }
 127
 128 inline  void  Vector::Normalize(void)
 129 {
 130     float m = (float) sqrt(x * x + y * y + z * z);
 131     if (m <= tol) m = 1;
 132     x /= m;
 133     y /= m;
 134     z /= m;
 135
 136     if (fabs(x) < tol) x = 0.0f;
 137     if (fabs(y) < tol) y = 0.0f;
 138     if (fabs(z) < tol) z = 0.0f;
 139 }
 140
 141 inline  void  Vector::Reverse(void)
 142 {
 143     x = -x;
 144     y = -y;
 145     z = -z;
 146 }
 147
 148 inline Vector& Vector::operator+=(Vector u)
 149 {
 150     x += u.x;
 151     y += u.y;
 152     z += u.z;
 153     return *this;
 154 }
 155
 156 inline  Vector& Vector::operator-=(Vector u)
 157 {
 158     x -= u.x;
 159     y -= u.y;
 160     z -= u.z;
 161     return *this;
 162 }
 163
 164 inline  Vector& Vector::operator*=(float s)
 165 {
 166     x *= s;
 167     y *= s;
 168     z *= s;
 169     return *this;
 170 }
 171
 172 inline  Vector& Vector::operator/=(float s)
 173 {
 174     x /= s;
 175     y /= s;
 176     z /= s;
 177     return *this;
 178 }
 179
 180 inline  Vector Vector::operator-(void)
 181 {
 182     return Vector(-x, -y, -z);
 183 }
 184
 185
 186 inline  Vector operator+(Vector u, Vector v)
 187 {
 188     return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
 189 }
 190
 191 inline  Vector operator-(Vector u, Vector v)
 192 {
 193     return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
 194 }
 195
 196 // Vector cross product (u cross v)
 197 inline  Vector operator^(Vector u, Vector v)
 198 {
 199     return Vector(      u.y * v.z - u.z * v.y,
 200                     -u.x * v.z + u.z * v.x,
 201                     u.x * v.y - u.y * v.x );
 202 }
 203
 204 // Vector dot product
 205 inline  float operator*(Vector u, Vector v)
 206 {
 207     return (u.x * v.x + u.y * v.y + u.z * v.z);
 208 }
 209
 210 inline  Vector operator*(float s, Vector u)
 211 {
 212     return Vector(u.x * s, u.y * s, u.z * s);
 213 }
 214
 215 inline  Vector operator*(Vector u, float s)
 216 {
 217     return Vector(u.x * s, u.y * s, u.z * s);
 218 }
 219
 220 inline  Vector operator/(Vector u, float s)
 221 {
 222     return Vector(u.x / s, u.y / s, u.z / s);
 223 }
 224
 225 // triple scalar product (u dot (v cross w))
 226 inline  float TripleScalarProduct(Vector u, Vector v, Vector w)
 227 {
 228     return float(       (u.x * (v.y * w.z - v.z * w.y)) +
 229                     (u.y * (-v.x * w.z + v.z * w.x)) +
 230                     (u.z * (v.x * w.y - v.y * w.x)) );
 231     //return u*(v^w);
 232
 233 }
 234
 235
 236
 237 //------------------------------------------------------------------------//
 238 // Matrix Class and matrix functions
 239 //------------------------------------------------------------------------//
 240
 241 class Matrix3x3
 242 {
 243 public:
 244     // elements eij: i -> row, j -> column
 245     float       e11, e12, e13, e21, e22, e23, e31, e32, e33;
 246
 247     Matrix3x3(void);
 248     Matrix3x3(  float r1c1, float r1c2, float r1c3,
 249                 float r2c1, float r2c2, float r2c3,
 250                 float r3c1, float r3c2, float r3c3 );
 251
 252     float       det(void);
 253     Matrix3x3   Transpose(void);
 254     Matrix3x3   Inverse(void);
 255
 256     Matrix3x3& operator+=(Matrix3x3 m);
 257     Matrix3x3& operator-=(Matrix3x3 m);
 258     Matrix3x3& operator*=(float s);
 259     Matrix3x3& operator/=(float s);
 260 };
 261
 262 inline  Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
 263 inline  Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
 264 inline  Matrix3x3 operator/(Matrix3x3 m, float s);
 265 inline  Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
 266 inline  Matrix3x3 operator*(Matrix3x3 m, float s);
 267 inline  Matrix3x3 operator*(float s, Matrix3x3 m);
 268 inline  Vector operator*(Matrix3x3 m, Vector u);
 269 inline  Vector operator*(Vector u, Matrix3x3 m);
 270
 271
 272
 273
 274
 275 inline  Matrix3x3::Matrix3x3(void)
 276 {
 277     e11 = 0;
 278     e12 = 0;
 279     e13 = 0;
 280     e21 = 0;
 281     e22 = 0;
 282     e23 = 0;
 283     e31 = 0;
 284     e32 = 0;
 285     e33 = 0;
 286 }
 287
 288 inline  Matrix3x3::Matrix3x3(   float r1c1, float r1c2, float r1c3,
 289                                 float r2c1, float r2c2, float r2c3,
 290                                 float r3c1, float r3c2, float r3c3 )
 291 {
 292     e11 = r1c1;
 293     e12 = r1c2;
 294     e13 = r1c3;
 295     e21 = r2c1;
 296     e22 = r2c2;
 297     e23 = r2c3;
 298     e31 = r3c1;
 299     e32 = r3c2;
 300     e33 = r3c3;
 301 }
 302
 303 inline  float   Matrix3x3::det(void)
 304 {
 305     return      e11 * e22 * e33 -
 306             e11 * e32 * e23 +
 307             e21 * e32 * e13 -
 308             e21 * e12 * e33 +
 309             e31 * e12 * e23 -
 310             e31 * e22 * e13;
 311 }
 312
 313 inline  Matrix3x3       Matrix3x3::Transpose(void)
 314 {
 315     return Matrix3x3(e11, e21, e31, e12, e22, e32, e13, e23, e33);
 316 }
 317
 318 inline  Matrix3x3       Matrix3x3::Inverse(void)
 319 {
 320     float       d = e11 * e22 * e33 -
 321                 e11 * e32 * e23 +
 322                 e21 * e32 * e13 -
 323                 e21 * e12 * e33 +
 324                 e31 * e12 * e23 -
 325                 e31 * e22 * e13;
 326
 327     if (d == 0) d = 1;
 328
 329     return      Matrix3x3(      (e22 * e33 - e23 * e32) / d,
 330                         -(e12 * e33 - e13 * e32) / d,
 331                         (e12 * e23 - e13 * e22) / d,
 332                         -(e21 * e33 - e23 * e31) / d,
 333                         (e11 * e33 - e13 * e31) / d,
 334                         -(e11 * e23 - e13 * e21) / d,
 335                         (e21 * e32 - e22 * e31) / d,
 336                         -(e11 * e32 - e12 * e31) / d,
 337                         (e11 * e22 - e12 * e21) / d );
 338 }
 339
 340 inline  Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
 341 {
 342     e11 += m.e11;
 343     e12 += m.e12;
 344     e13 += m.e13;
 345     e21 += m.e21;
 346     e22 += m.e22;
 347     e23 += m.e23;
 348     e31 += m.e31;
 349     e32 += m.e32;
 350     e33 += m.e33;
 351     return *this;
 352 }
 353
 354 inline  Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
 355 {
 356     e11 -= m.e11;
 357     e12 -= m.e12;
 358     e13 -= m.e13;
 359     e21 -= m.e21;
 360     e22 -= m.e22;
 361     e23 -= m.e23;
 362     e31 -= m.e31;
 363     e32 -= m.e32;
 364     e33 -= m.e33;
 365     return *this;
 366 }
 367
 368 inline  Matrix3x3& Matrix3x3::operator*=(float s)
 369 {
 370     e11 *= s;
 371     e12 *= s;
 372     e13 *= s;
 373     e21 *= s;
 374     e22 *= s;
 375     e23 *= s;
 376     e31 *= s;
 377     e32 *= s;
 378     e33 *= s;
 379     return *this;
 380 }
 381
 382 inline  Matrix3x3& Matrix3x3::operator/=(float s)
 383 {
 384     e11 /= s;
 385     e12 /= s;
 386     e13 /= s;
 387     e21 /= s;
 388     e22 /= s;
 389     e23 /= s;
 390     e31 /= s;
 391     e32 /= s;
 392     e33 /= s;
 393     return *this;
 394 }
 395
 396 inline  Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
 397 {
 398     return      Matrix3x3(      m1.e11 + m2.e11,
 399                         m1.e12 + m2.e12,
 400                         m1.e13 + m2.e13,
 401                         m1.e21 + m2.e21,
 402                         m1.e22 + m2.e22,
 403                         m1.e23 + m2.e23,
 404                         m1.e31 + m2.e31,
 405                         m1.e32 + m2.e32,
 406                         m1.e33 + m2.e33);
 407 }
 408
 409 inline  Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2)
 410 {
 411     return      Matrix3x3(      m1.e11 - m2.e11,
 412                         m1.e12 - m2.e12,
 413                         m1.e13 - m2.e13,
 414                         m1.e21 - m2.e21,
 415                         m1.e22 - m2.e22,
 416                         m1.e23 - m2.e23,
 417                         m1.e31 - m2.e31,
 418                         m1.e32 - m2.e32,
 419                         m1.e33 - m2.e33);
 420 }
 421
 422 inline  Matrix3x3 operator/(Matrix3x3 m, float s)
 423 {
 424     return      Matrix3x3(      m.e11 / s,
 425                         m.e12 / s,
 426                         m.e13 / s,
 427                         m.e21 / s,
 428                         m.e22 / s,
 429                         m.e23 / s,
 430                         m.e31 / s,
 431                         m.e32 / s,
 432                         m.e33 / s);
 433 }
 434
 435 inline  Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2)
 436 {
 437     return Matrix3x3(   m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31,
 438                         m1.e11 * m2.e12 + m1.e12 * m2.e22 + m1.e13 * m2.e32,
 439                         m1.e11 * m2.e13 + m1.e12 * m2.e23 + m1.e13 * m2.e33,
 440                         m1.e21 * m2.e11 + m1.e22 * m2.e21 + m1.e23 * m2.e31,
 441                         m1.e21 * m2.e12 + m1.e22 * m2.e22 + m1.e23 * m2.e32,
 442                         m1.e21 * m2.e13 + m1.e22 * m2.e23 + m1.e23 * m2.e33,
 443                         m1.e31 * m2.e11 + m1.e32 * m2.e21 + m1.e33 * m2.e31,
 444                         m1.e31 * m2.e12 + m1.e32 * m2.e22 + m1.e33 * m2.e32,
 445                         m1.e31 * m2.e13 + m1.e32 * m2.e23 + m1.e33 * m2.e33 );
 446 }
 447
 448 inline  Matrix3x3 operator*(Matrix3x3 m, float s)
 449 {
 450     return      Matrix3x3(      m.e11 * s,
 451                         m.e12 * s,
 452                         m.e13 * s,
 453                         m.e21 * s,
 454                         m.e22 * s,
 455                         m.e23 * s,
 456                         m.e31 * s,
 457                         m.e32 * s,
 458                         m.e33 * s);
 459 }
 460
 461 inline  Matrix3x3 operator*(float s, Matrix3x3 m)
 462 {
 463     return      Matrix3x3(      m.e11 * s,
 464                         m.e12 * s,
 465                         m.e13 * s,
 466                         m.e21 * s,
 467                         m.e22 * s,
 468                         m.e23 * s,
 469                         m.e31 * s,
 470                         m.e32 * s,
 471                         m.e33 * s);
 472 }
 473
 474 inline  Vector operator*(Matrix3x3 m, Vector u)
 475 {
 476     return Vector(      m.e11 * u.x + m.e12 * u.y + m.e13 * u.z,
 477                     m.e21 * u.x + m.e22 * u.y + m.e23 * u.z,
 478                     m.e31 * u.x + m.e32 * u.y + m.e33 * u.z);
 479 }
 480
 481 inline  Vector operator*(Vector u, Matrix3x3 m)
 482 {
 483     return Vector(      u.x * m.e11 + u.y * m.e21 + u.z * m.e31,
 484                     u.x * m.e12 + u.y * m.e22 + u.z * m.e32,
 485                     u.x * m.e13 + u.y * m.e23 + u.z * m.e33);
 486 }
 487
 488 //------------------------------------------------------------------------//
 489 // Quaternion Class and Quaternion functions
 490 //------------------------------------------------------------------------//
 491
 492 class Quaternion
 493 {
 494 public:
 495     float       n;      // number (scalar) part
 496     Vector      v;      // vector part: v.x, v.y, v.z
 497
 498     Quaternion(void);
 499     Quaternion(float e0, float e1, float e2, float e3);
 500
 501     float       Magnitude(void);
 502     Vector      GetVector(void);
 503     float       GetScalar(void);
 504     Quaternion  operator+=(Quaternion q);
 505     Quaternion  operator-=(Quaternion q);
 506     Quaternion operator*=(float s);
 507     Quaternion operator/=(float s);
 508     Quaternion  operator~(void) const {
 509         return Quaternion(n, -v.x, -v.y, -v.z);
 510     }
 511 };
 512
 513 inline  Quaternion operator+(Quaternion q1, Quaternion q2);
 514 inline  Quaternion operator-(Quaternion q1, Quaternion q2);
 515 inline  Quaternion operator*(Quaternion q1, Quaternion q2);
 516 inline  Quaternion operator*(Quaternion q, float s);
 517 inline  Quaternion operator*(float s, Quaternion q);
 518 inline  Quaternion operator*(Quaternion q, Vector v);
 519 inline  Quaternion operator*(Vector v, Quaternion q);
 520 inline  Quaternion operator/(Quaternion q, float s);
 521 inline  float QGetAngle(Quaternion q);
 522 inline  Vector QGetAxis(Quaternion q);
 523 inline  Quaternion QRotate(Quaternion q1, Quaternion q2);
 524 inline  Vector  QVRotate(Quaternion q, Vector v);
 525 inline  Quaternion      MakeQFromEulerAngles(float x, float y, float z);
 526 inline  Vector  MakeEulerAnglesFromQ(Quaternion q);
 527
 528
 529 inline  Quaternion::Quaternion(void)
 530 {
 531     n = 0;
 532     v.x = 0;
 533     v.y =  0;
 534     v.z = 0;
 535 }
 536
 537 inline  Quaternion::Quaternion(float e0, float e1, float e2, float e3)
 538 {
 539     n = e0;
 540     v.x = e1;
 541     v.y = e2;
 542     v.z = e3;
 543 }
 544
 545 inline  float   Quaternion::Magnitude(void)
 546 {
 547     return (float) sqrt(n * n + v.x * v.x + v.y * v.y + v.z * v.z);
 548 }
 549
 550 inline  Vector  Quaternion::GetVector(void)
 551 {
 552     return Vector(v.x, v.y, v.z);
 553 }
 554
 555 inline  float   Quaternion::GetScalar(void)
 556 {
 557     return n;
 558 }
 559
 560 inline  Quaternion      Quaternion::operator+=(Quaternion q)
 561 {
 562     n += q.n;
 563     v.x += q.v.x;
 564     v.y += q.v.y;
 565     v.z += q.v.z;
 566     return *this;
 567 }
 568
 569 inline  Quaternion      Quaternion::operator-=(Quaternion q)
 570 {
 571     n -= q.n;
 572     v.x -= q.v.x;
 573     v.y -= q.v.y;
 574     v.z -= q.v.z;
 575     return *this;
 576 }
 577
 578 inline  Quaternion Quaternion::operator*=(float s)
 579 {
 580     n *= s;
 581     v.x *= s;
 582     v.y *= s;
 583     v.z *= s;
 584     return *this;
 585 }
 586
 587 inline  Quaternion Quaternion::operator/=(float s)
 588 {
 589     n /= s;
 590     v.x /= s;
 591     v.y /= s;
 592     v.z /= s;
 593     return *this;
 594 }
 595
 596 /*inline        Quaternion      Quaternion::operator~()
 597 {
 598 return Quaternion(n, -v.x, -v.y, -v.z);
 599 }*/
 600
 601 inline  Quaternion operator+(Quaternion q1, Quaternion q2)
 602 {
 603     return      Quaternion(     q1.n + q2.n,
 604                         q1.v.x + q2.v.x,
 605                         q1.v.y + q2.v.y,
 606                         q1.v.z + q2.v.z);
 607 }
 608
 609 inline  Quaternion operator-(Quaternion q1, Quaternion q2)
 610 {
 611     return      Quaternion(     q1.n - q2.n,
 612                         q1.v.x - q2.v.x,
 613                         q1.v.y - q2.v.y,
 614                         q1.v.z - q2.v.z);
 615 }
 616
 617 inline  Quaternion operator*(Quaternion q1, Quaternion q2)
 618 {
 619     return      Quaternion(     q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y - q1.v.z * q2.v.z,
 620                         q1.n * q2.v.x + q1.v.x * q2.n + q1.v.y * q2.v.z - q1.v.z * q2.v.y,
 621                         q1.n * q2.v.y + q1.v.y * q2.n + q1.v.z * q2.v.x - q1.v.x * q2.v.z,
 622                         q1.n * q2.v.z + q1.v.z * q2.n + q1.v.x * q2.v.y - q1.v.y * q2.v.x);
 623 }
 624
 625 inline  Quaternion operator*(Quaternion q, float s)
 626 {
 627     return      Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
 628 }
 629
 630 inline  Quaternion operator*(float s, Quaternion q)
 631 {
 632     return      Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
 633 }
 634
 635 inline  Quaternion operator*(Quaternion q, Vector v)
 636 {
 637     return      Quaternion(     -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
 638                         q.n * v.x + q.v.y * v.z - q.v.z * v.y,
 639                         q.n * v.y + q.v.z * v.x - q.v.x * v.z,
 640                         q.n * v.z + q.v.x * v.y - q.v.y * v.x);
 641 }
 642
 643 inline  Quaternion operator*(Vector v, Quaternion q)
 644 {
 645     return      Quaternion(     -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
 646                         q.n * v.x + q.v.z * v.y - q.v.y * v.z,
 647                         q.n * v.y + q.v.x * v.z - q.v.z * v.x,
 648                         q.n * v.z + q.v.y * v.x - q.v.x * v.y);
 649 }
 650
 651 inline  Quaternion operator/(Quaternion q, float s)
 652 {
 653     return      Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s);
 654 }
 655
 656 inline  float QGetAngle(Quaternion q)
 657 {
 658     return      (float) (2 * acosf(q.n));
 659 }
 660
 661 inline  Vector QGetAxis(Quaternion q)
 662 {
 663     Vector v;
 664     float m;
 665
 666     v = q.GetVector();
 667     m = v.Magnitude();
 668
 669     if (m <= tol)
 670         return Vector();
 671     else
 672         return v / m;
 673 }
 674
 675 inline  Quaternion QRotate(Quaternion q1, Quaternion q2)
 676 {
 677     return      q1 * q2 * (~q1);
 678 }
 679
 680 inline  Vector  QVRotate(Quaternion q, Vector v)
 681 {
 682     Quaternion t;
 683
 684
 685     t = q * v * (~q);
 686
 687     return      t.GetVector();
 688 }
 689
 690 inline  Quaternion      MakeQFromEulerAngles(float x, float y, float z)
 691 {
 692     Quaternion  q;
 693     double      roll = DegreesToRadians(x);
 694     double      pitch = DegreesToRadians(y);
 695     double      yaw = DegreesToRadians(z);
 696
 697     double      cyaw, cpitch, croll, syaw, spitch, sroll;
 698     double      cyawcpitch, syawspitch, cyawspitch, syawcpitch;
 699
 700     cyaw = cos(0.5f * yaw);
 701     cpitch = cos(0.5f * pitch);
 702     croll = cos(0.5f * roll);
 703     syaw = sin(0.5f * yaw);
 704     spitch = sin(0.5f * pitch);
 705     sroll = sin(0.5f * roll);
 706
 707     cyawcpitch = cyaw * cpitch;
 708     syawspitch = syaw * spitch;
 709     cyawspitch = cyaw * spitch;
 710     syawcpitch = syaw * cpitch;
 711
 712     q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
 713     q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
 714     q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
 715     q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
 716
 717     return q;
 718 }
 719
 720 inline  Vector  MakeEulerAnglesFromQ(Quaternion q)
 721 {
 722     double      r11, r21, r31, r32, r33;
 723     double      q00, q11, q22, q33;
 724     double      tmp;
 725     Vector      u;
 726
 727     q00 = q.n * q.n;
 728     q11 = q.v.x * q.v.x;
 729     q22 = q.v.y * q.v.y;
 730     q33 = q.v.z * q.v.z;
 731
 732     r11 = q00 + q11 - q22 - q33;
 733     r21 = 2 * (q.v.x * q.v.y + q.n * q.v.z);
 734     r31 = 2 * (q.v.x * q.v.z - q.n * q.v.y);
 735     r32 = 2 * (q.v.y * q.v.z + q.n * q.v.x);
 736     r33 = q00 - q11 - q22 + q33;
 737
 738     tmp = fabs(r31);
 739     if (tmp > 0.999999) {
 740         double r12 = 2 * (q.v.x * q.v.y - q.n * q.v.z);
 741         double r13 = 2 * (q.v.x * q.v.z + q.n * q.v.y);
 742
 743         u.x = RadiansToDegrees(0.0f); //roll
 744         u.y = RadiansToDegrees((float) (-(pi / 2) * r31 / tmp)); // pitch
 745         u.z = RadiansToDegrees((float) atan2(-r12, -r31 * r13)); // yaw
 746         return u;
 747     }
 748
 749     u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
 750     u.y = RadiansToDegrees((float) asinf(-r31));                 // pitch
 751     u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw
 752     return u;
 753
 754
 755 }
 756
 757
 758
 759
 760
 761 #endif