Source/PhysicsMath.h

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