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