Source/PhysicsMath.h

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