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