Source/Math/PhysicsMath.hpp

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