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)
 132         m = 1;
 133     x /= m;
 134     y /= m;
 135     z /= m;
 136
 137     if (fabs(x) < tol)
 138         x = 0.0f;
 139     if (fabs(y) < tol)
 140         y = 0.0f;
 141     if (fabs(z) < tol)
 142         z = 0.0f;
 143 }
 144
 145 inline void  Vector::Reverse(void)
 146 {
 147     x = -x;
 148     y = -y;
 149     z = -z;
 150 }
 151
 152 inline Vector& Vector::operator+=(Vector u)
 153 {
 154     x += u.x;
 155     y += u.y;
 156     z += u.z;
 157     return *this;
 158 }
 159
 160 inline Vector& Vector::operator-=(Vector u)
 161 {
 162     x -= u.x;
 163     y -= u.y;
 164     z -= u.z;
 165     return *this;
 166 }
 167
 168 inline Vector& Vector::operator*=(float s)
 169 {
 170     x *= s;
 171     y *= s;
 172     z *= s;
 173     return *this;
 174 }
 175
 176 inline Vector& Vector::operator/=(float s)
 177 {
 178     x /= s;
 179     y /= s;
 180     z /= s;
 181     return *this;
 182 }
 183
 184 inline Vector Vector::operator-(void)
 185 {
 186     return Vector(-x, -y, -z);
 187 }
 188
 189
 190 inline Vector operator+(Vector u, Vector v)
 191 {
 192     return Vector(u.x + v.x, u.y + v.y, u.z + v.z);
 193 }
 194
 195 inline Vector operator-(Vector u, Vector v)
 196 {
 197     return Vector(u.x - v.x, u.y - v.y, u.z - v.z);
 198 }
 199
 200 // Vector cross product (u cross v)
 201 inline Vector operator^(Vector u, Vector v)
 202 {
 203     return Vector(  u.y * v.z - u.z * v.y,
 204                     -u.x * v.z + u.z * v.x,
 205                     u.x * v.y - u.y * v.x );
 206 }
 207
 208 // Vector dot product
 209 inline float operator*(Vector u, Vector v)
 210 {
 211     return (u.x * v.x + u.y * v.y + u.z * v.z);
 212 }
 213
 214 inline Vector operator*(float s, Vector u)
 215 {
 216     return Vector(u.x * s, u.y * s, u.z * s);
 217 }
 218
 219 inline Vector operator*(Vector u, float s)
 220 {
 221     return Vector(u.x * s, u.y * s, u.z * s);
 222 }
 223
 224 inline Vector operator/(Vector u, float s)
 225 {
 226     return Vector(u.x / s, u.y / s, u.z / s);
 227 }
 228
 229 // triple scalar product (u dot (v cross w))
 230 inline float TripleScalarProduct(Vector u, Vector v, Vector w)
 231 {
 232     return float(   (u.x * (v.y * w.z - v.z * w.y)) +
 233                     (u.y * (-v.x * w.z + v.z * w.x)) +
 234                     (u.z * (v.x * w.y - v.y * w.x)) );
 235     //return u*(v^w);
 236
 237 }
 238
 239
 240
 241 //------------------------------------------------------------------------//
 242 // Matrix Class and matrix functions
 243 //------------------------------------------------------------------------//
 244
 245 class Matrix3x3
 246 {
 247 public:
 248     // elements eij: i -> row, j -> column
 249     float e11, e12, e13, e21, e22, e23, e31, e32, e33;
 250
 251     Matrix3x3(void);
 252     Matrix3x3(  float r1c1, float r1c2, float r1c3,
 253                 float r2c1, float r2c2, float r2c3,
 254                 float r3c1, float r3c2, float r3c3 );
 255
 256     float det(void);
 257     Matrix3x3 Transpose(void);
 258     Matrix3x3 Inverse(void);
 259
 260     Matrix3x3& operator+=(Matrix3x3 m);
 261     Matrix3x3& operator-=(Matrix3x3 m);
 262     Matrix3x3& operator*=(float s);
 263     Matrix3x3& operator/=(float s);
 264 };
 265
 266 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2);
 267 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2);
 268 inline Matrix3x3 operator/(Matrix3x3 m, float s);
 269 inline Matrix3x3 operator*(Matrix3x3 m1, Matrix3x3 m2);
 270 inline Matrix3x3 operator*(Matrix3x3 m, float s);
 271 inline Matrix3x3 operator*(float s, Matrix3x3 m);
 272 inline Vector operator*(Matrix3x3 m, Vector u);
 273 inline Vector operator*(Vector u, Matrix3x3 m);
 274
 275
 276
 277
 278
 279 inline Matrix3x3::Matrix3x3(void)
 280 {
 281     e11 = 0;
 282     e12 = 0;
 283     e13 = 0;
 284     e21 = 0;
 285     e22 = 0;
 286     e23 = 0;
 287     e31 = 0;
 288     e32 = 0;
 289     e33 = 0;
 290 }
 291
 292 inline Matrix3x3::Matrix3x3(    float r1c1, float r1c2, float r1c3,
 293                                 float r2c1, float r2c2, float r2c3,
 294                                 float r3c1, float r3c2, float r3c3 )
 295 {
 296     e11 = r1c1;
 297     e12 = r1c2;
 298     e13 = r1c3;
 299     e21 = r2c1;
 300     e22 = r2c2;
 301     e23 = r2c3;
 302     e31 = r3c1;
 303     e32 = r3c2;
 304     e33 = r3c3;
 305 }
 306
 307 inline float Matrix3x3::det(void)
 308 {
 309     return  e11 * e22 * e33 -
 310             e11 * e32 * e23 +
 311             e21 * e32 * e13 -
 312             e21 * e12 * e33 +
 313             e31 * e12 * e23 -
 314             e31 * e22 * e13;
 315 }
 316
 317 inline Matrix3x3 Matrix3x3::Transpose(void)
 318 {
 319     return Matrix3x3(e11, e21, e31, e12, e22, e32, e13, e23, e33);
 320 }
 321
 322 inline Matrix3x3 Matrix3x3::Inverse(void)
 323 {
 324     float d =   e11 * e22 * e33 -
 325                 e11 * e32 * e23 +
 326                 e21 * e32 * e13 -
 327                 e21 * e12 * e33 +
 328                 e31 * e12 * e23 -
 329                 e31 * e22 * e13;
 330
 331     if (d == 0)
 332         d = 1;
 333
 334     return Matrix3x3(   (e22 * e33 - e23 * e32) / d,
 335                         -(e12 * e33 - e13 * e32) / d,
 336                         (e12 * e23 - e13 * e22) / d,
 337                         -(e21 * e33 - e23 * e31) / d,
 338                         (e11 * e33 - e13 * e31) / d,
 339                         -(e11 * e23 - e13 * e21) / d,
 340                         (e21 * e32 - e22 * e31) / d,
 341                         -(e11 * e32 - e12 * e31) / d,
 342                         (e11 * e22 - e12 * e21) / d );
 343 }
 344
 345 inline Matrix3x3& Matrix3x3::operator+=(Matrix3x3 m)
 346 {
 347     e11 += m.e11;
 348     e12 += m.e12;
 349     e13 += m.e13;
 350     e21 += m.e21;
 351     e22 += m.e22;
 352     e23 += m.e23;
 353     e31 += m.e31;
 354     e32 += m.e32;
 355     e33 += m.e33;
 356     return *this;
 357 }
 358
 359 inline Matrix3x3& Matrix3x3::operator-=(Matrix3x3 m)
 360 {
 361     e11 -= m.e11;
 362     e12 -= m.e12;
 363     e13 -= m.e13;
 364     e21 -= m.e21;
 365     e22 -= m.e22;
 366     e23 -= m.e23;
 367     e31 -= m.e31;
 368     e32 -= m.e32;
 369     e33 -= m.e33;
 370     return *this;
 371 }
 372
 373 inline Matrix3x3& Matrix3x3::operator*=(float s)
 374 {
 375     e11 *= s;
 376     e12 *= s;
 377     e13 *= s;
 378     e21 *= s;
 379     e22 *= s;
 380     e23 *= s;
 381     e31 *= s;
 382     e32 *= s;
 383     e33 *= s;
 384     return *this;
 385 }
 386
 387 inline Matrix3x3& Matrix3x3::operator/=(float s)
 388 {
 389     e11 /= s;
 390     e12 /= s;
 391     e13 /= s;
 392     e21 /= s;
 393     e22 /= s;
 394     e23 /= s;
 395     e31 /= s;
 396     e32 /= s;
 397     e33 /= s;
 398     return *this;
 399 }
 400
 401 inline Matrix3x3 operator+(Matrix3x3 m1, Matrix3x3 m2)
 402 {
 403     return Matrix3x3(   m1.e11 + m2.e11,
 404                         m1.e12 + m2.e12,
 405                         m1.e13 + m2.e13,
 406                         m1.e21 + m2.e21,
 407                         m1.e22 + m2.e22,
 408                         m1.e23 + m2.e23,
 409                         m1.e31 + m2.e31,
 410                         m1.e32 + m2.e32,
 411                         m1.e33 + m2.e33);
 412 }
 413
 414 inline Matrix3x3 operator-(Matrix3x3 m1, Matrix3x3 m2)
 415 {
 416     return Matrix3x3(   m1.e11 - m2.e11,
 417                         m1.e12 - m2.e12,
 418                         m1.e13 - m2.e13,
 419                         m1.e21 - m2.e21,
 420                         m1.e22 - m2.e22,
 421                         m1.e23 - m2.e23,
 422                         m1.e31 - m2.e31,
 423                         m1.e32 - m2.e32,
 424                         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*(Matrix3x3 m1, Matrix3x3 m2)
 441 {
 442     return Matrix3x3(   m1.e11 * m2.e11 + m1.e12 * m2.e21 + m1.e13 * m2.e31,
 443                         m1.e11 * m2.e12 + m1.e12 * m2.e22 + m1.e13 * m2.e32,
 444                         m1.e11 * m2.e13 + m1.e12 * m2.e23 + m1.e13 * m2.e33,
 445                         m1.e21 * m2.e11 + m1.e22 * m2.e21 + m1.e23 * m2.e31,
 446                         m1.e21 * m2.e12 + m1.e22 * m2.e22 + m1.e23 * m2.e32,
 447                         m1.e21 * m2.e13 + m1.e22 * m2.e23 + m1.e23 * m2.e33,
 448                         m1.e31 * m2.e11 + m1.e32 * m2.e21 + m1.e33 * m2.e31,
 449                         m1.e31 * m2.e12 + m1.e32 * m2.e22 + m1.e33 * m2.e32,
 450                         m1.e31 * m2.e13 + m1.e32 * m2.e23 + m1.e33 * m2.e33 );
 451 }
 452
 453 inline Matrix3x3 operator*(Matrix3x3 m, float s)
 454 {
 455     return Matrix3x3(   m.e11 * s,
 456                         m.e12 * s,
 457                         m.e13 * s,
 458                         m.e21 * s,
 459                         m.e22 * s,
 460                         m.e23 * s,
 461                         m.e31 * s,
 462                         m.e32 * s,
 463                         m.e33 * s);
 464 }
 465
 466 inline Matrix3x3 operator*(float s, Matrix3x3 m)
 467 {
 468     return Matrix3x3(   m.e11 * s,
 469                         m.e12 * s,
 470                         m.e13 * s,
 471                         m.e21 * s,
 472                         m.e22 * s,
 473                         m.e23 * s,
 474                         m.e31 * s,
 475                         m.e32 * s,
 476                         m.e33 * s);
 477 }
 478
 479 inline Vector operator*(Matrix3x3 m, Vector u)
 480 {
 481     return Vector(  m.e11 * u.x + m.e12 * u.y + m.e13 * u.z,
 482                     m.e21 * u.x + m.e22 * u.y + m.e23 * u.z,
 483                     m.e31 * u.x + m.e32 * u.y + m.e33 * u.z);
 484 }
 485
 486 inline Vector operator*(Vector u, Matrix3x3 m)
 487 {
 488     return Vector(  u.x * m.e11 + u.y * m.e21 + u.z * m.e31,
 489                     u.x * m.e12 + u.y * m.e22 + u.z * m.e32,
 490                     u.x * m.e13 + u.y * m.e23 + u.z * m.e33);
 491 }
 492
 493 //------------------------------------------------------------------------//
 494 // Quaternion Class and Quaternion functions
 495 //------------------------------------------------------------------------//
 496
 497 class Quaternion
 498 {
 499 public:
 500     float n; // number (scalar) part
 501     Vector v; // vector part: v.x, v.y, v.z
 502
 503     Quaternion(void);
 504     Quaternion(float e0, float e1, float e2, float e3);
 505
 506     float Magnitude(void);
 507     Vector GetVector(void);
 508     float GetScalar(void);
 509     Quaternion operator+=(Quaternion q);
 510     Quaternion operator-=(Quaternion q);
 511     Quaternion operator*=(float s);
 512     Quaternion operator/=(float s);
 513     Quaternion operator~(void) const {
 514         return Quaternion(n, -v.x, -v.y, -v.z);
 515     }
 516 };
 517
 518 inline Quaternion operator+(Quaternion q1, Quaternion q2);
 519 inline Quaternion operator-(Quaternion q1, Quaternion q2);
 520 inline Quaternion operator*(Quaternion q1, Quaternion q2);
 521 inline Quaternion operator*(Quaternion q, float s);
 522 inline Quaternion operator*(float s, Quaternion q);
 523 inline Quaternion operator*(Quaternion q, Vector v);
 524 inline Quaternion operator*(Vector v, Quaternion q);
 525 inline Quaternion operator/(Quaternion q, float s);
 526 inline float QGetAngle(Quaternion q);
 527 inline Vector QGetAxis(Quaternion q);
 528 inline Quaternion QRotate(Quaternion q1, Quaternion q2);
 529 inline Vector QVRotate(Quaternion q, Vector v);
 530 inline Quaternion MakeQFromEulerAngles(float x, float y, float z);
 531 inline Vector MakeEulerAnglesFromQ(Quaternion q);
 532
 533
 534 inline Quaternion::Quaternion(void)
 535 {
 536     n = 0;
 537     v.x = 0;
 538     v.y =  0;
 539     v.z = 0;
 540 }
 541
 542 inline Quaternion::Quaternion(float e0, float e1, float e2, float e3)
 543 {
 544     n = e0;
 545     v.x = e1;
 546     v.y = e2;
 547     v.z = e3;
 548 }
 549
 550 inline float Quaternion::Magnitude(void)
 551 {
 552     return (float) sqrt(n * n + v.x * v.x + v.y * v.y + v.z * v.z);
 553 }
 554
 555 inline Vector Quaternion::GetVector(void)
 556 {
 557     return Vector(v.x, v.y, v.z);
 558 }
 559
 560 inline float Quaternion::GetScalar(void)
 561 {
 562     return n;
 563 }
 564
 565 inline Quaternion Quaternion::operator+=(Quaternion q)
 566 {
 567     n += q.n;
 568     v.x += q.v.x;
 569     v.y += q.v.y;
 570     v.z += q.v.z;
 571     return *this;
 572 }
 573
 574 inline Quaternion Quaternion::operator-=(Quaternion q)
 575 {
 576     n -= q.n;
 577     v.x -= q.v.x;
 578     v.y -= q.v.y;
 579     v.z -= q.v.z;
 580     return *this;
 581 }
 582
 583 inline Quaternion Quaternion::operator*=(float s)
 584 {
 585     n *= s;
 586     v.x *= s;
 587     v.y *= s;
 588     v.z *= s;
 589     return *this;
 590 }
 591
 592 inline Quaternion Quaternion::operator/=(float s)
 593 {
 594     n /= s;
 595     v.x /= s;
 596     v.y /= s;
 597     v.z /= s;
 598     return *this;
 599 }
 600
 601 /*inline Quaternion Quaternion::operator~()
 602 {
 603 return Quaternion(n, -v.x, -v.y, -v.z);
 604 }*/
 605
 606 inline Quaternion operator+(Quaternion q1, Quaternion q2)
 607 {
 608     return Quaternion(  q1.n + q2.n,
 609                         q1.v.x + q2.v.x,
 610                         q1.v.y + q2.v.y,
 611                         q1.v.z + q2.v.z);
 612 }
 613
 614 inline Quaternion operator-(Quaternion q1, Quaternion q2)
 615 {
 616     return Quaternion(  q1.n - q2.n,
 617                         q1.v.x - q2.v.x,
 618                         q1.v.y - q2.v.y,
 619                         q1.v.z - q2.v.z);
 620 }
 621
 622 inline Quaternion operator*(Quaternion q1, Quaternion q2)
 623 {
 624     return Quaternion(  q1.n * q2.n - q1.v.x * q2.v.x - q1.v.y * q2.v.y - q1.v.z * q2.v.z,
 625                         q1.n * q2.v.x + q1.v.x * q2.n + q1.v.y * q2.v.z - q1.v.z * q2.v.y,
 626                         q1.n * q2.v.y + q1.v.y * q2.n + q1.v.z * q2.v.x - q1.v.x * q2.v.z,
 627                         q1.n * q2.v.z + q1.v.z * q2.n + q1.v.x * q2.v.y - q1.v.y * q2.v.x);
 628 }
 629
 630 inline Quaternion operator*(Quaternion q, float s)
 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*(float s, Quaternion q)
 636 {
 637     return Quaternion(q.n * s, q.v.x * s, q.v.y * s, q.v.z * s);
 638 }
 639
 640 inline Quaternion operator*(Quaternion q, Vector v)
 641 {
 642     return Quaternion(  -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
 643                         q.n * v.x + q.v.y * v.z - q.v.z * v.y,
 644                         q.n * v.y + q.v.z * v.x - q.v.x * v.z,
 645                         q.n * v.z + q.v.x * v.y - q.v.y * v.x);
 646 }
 647
 648 inline Quaternion operator*(Vector v, Quaternion q)
 649 {
 650     return Quaternion(  -(q.v.x * v.x + q.v.y * v.y + q.v.z * v.z),
 651                         q.n * v.x + q.v.z * v.y - q.v.y * v.z,
 652                         q.n * v.y + q.v.x * v.z - q.v.z * v.x,
 653                         q.n * v.z + q.v.y * v.x - q.v.x * v.y);
 654 }
 655
 656 inline Quaternion operator/(Quaternion q, float s)
 657 {
 658     return Quaternion(q.n / s, q.v.x / s, q.v.y / s, q.v.z / s);
 659 }
 660
 661 inline float QGetAngle(Quaternion q)
 662 {
 663     return (float) (2 * acosf(q.n));
 664 }
 665
 666 inline Vector QGetAxis(Quaternion q)
 667 {
 668     Vector v;
 669     float m;
 670
 671     v = q.GetVector();
 672     m = v.Magnitude();
 673
 674     if (m <= tol)
 675         return Vector();
 676     else
 677         return v / m;
 678 }
 679
 680 inline Quaternion QRotate(Quaternion q1, Quaternion q2)
 681 {
 682     return q1 * q2 * (~q1);
 683 }
 684
 685 inline Vector QVRotate(Quaternion q, Vector v)
 686 {
 687     Quaternion t;
 688
 689
 690     t = q * v * (~q);
 691
 692     return t.GetVector();
 693 }
 694
 695 inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
 696 {
 697     Quaternion q;
 698     double roll = DegreesToRadians(x);
 699     double pitch = DegreesToRadians(y);
 700     double yaw = DegreesToRadians(z);
 701
 702     double cyaw, cpitch, croll, syaw, spitch, sroll;
 703     double cyawcpitch, syawspitch, cyawspitch, syawcpitch;
 704
 705     cyaw = cos(0.5f * yaw);
 706     cpitch = cos(0.5f * pitch);
 707     croll = cos(0.5f * roll);
 708     syaw = sin(0.5f * yaw);
 709     spitch = sin(0.5f * pitch);
 710     sroll = sin(0.5f * roll);
 711
 712     cyawcpitch = cyaw * cpitch;
 713     syawspitch = syaw * spitch;
 714     cyawspitch = cyaw * spitch;
 715     syawcpitch = syaw * cpitch;
 716
 717     q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
 718     q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
 719     q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
 720     q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);
 721
 722     return q;
 723 }
 724
 725 inline Vector MakeEulerAnglesFromQ(Quaternion q)
 726 {
 727     double r11, r21, r31, r32, r33;
 728     double q00, q11, q22, q33;
 729     double tmp;
 730     Vector u;
 731
 732     q00 = q.n * q.n;
 733     q11 = q.v.x * q.v.x;
 734     q22 = q.v.y * q.v.y;
 735     q33 = q.v.z * q.v.z;
 736
 737     r11 = q00 + q11 - q22 - q33;
 738     r21 = 2 * (q.v.x * q.v.y + q.n * q.v.z);
 739     r31 = 2 * (q.v.x * q.v.z - q.n * q.v.y);
 740     r32 = 2 * (q.v.y * q.v.z + q.n * q.v.x);
 741     r33 = q00 - q11 - q22 + q33;
 742
 743     tmp = fabs(r31);
 744     if (tmp > 0.999999) {
 745         double r12 = 2 * (q.v.x * q.v.y - q.n * q.v.z);
 746         double r13 = 2 * (q.v.x * q.v.z + q.n * q.v.y);
 747
 748         u.x = RadiansToDegrees(0.0f); //roll
 749         u.y = RadiansToDegrees((float) (-(pi / 2) * r31 / tmp)); // pitch
 750         u.z = RadiansToDegrees((float) atan2(-r12, -r31 * r13)); // yaw
 751         return u;
 752     }
 753
 754     u.x = RadiansToDegrees((float) atan2(r32, r33)); // roll
 755     u.y = RadiansToDegrees((float) asinf(-r31));     // pitch
 756     u.z = RadiansToDegrees((float) atan2(r21, r11)); // yaw
 757     return u;
 758
 759
 760 }
 761
 762
 763
 764
 765
 766 #endif