Source/PhysicsMath.h

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