neo/idlib/math/Quat.h

   1 /*
   2 ===========================================================================
   3
   4 Doom 3 GPL Source Code
   5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
   6
   7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
   8
   9 Doom 3 Source Code is free software: you can redistribute it and/or modify
  10 it under the terms of the GNU General Public License as published by
  11 the Free Software Foundation, either version 3 of the License, or
  12 (at your option) any later version.
  13
  14 Doom 3 Source Code is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 GNU General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License
  20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
  21
  22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
  23
  24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  25
  26 ===========================================================================
  27 */
  28
  29 #ifndef __MATH_QUAT_H__
  30 #define __MATH_QUAT_H__
  31
  32 /*
  33 ===============================================================================
  34
  35         Quaternion
  36
  37 ===============================================================================
  38 */
  39
  40
  41 class idVec3;
  42 class idAngles;
  43 class idRotation;
  44 class idMat3;
  45 class idMat4;
  46 class idCQuat;
  47
  48 class idQuat {
  49 public:
  50         float                   x;
  51         float                   y;
  52         float                   z;
  53         float                   w;
  54
  55                                         idQuat( void );
  56                                         idQuat( float x, float y, float z, float w );
  57
  58         void                    Set( float x, float y, float z, float w );
  59
  60         float                   operator[]( int index ) const;
  61         float &                 operator[]( int index );
  62         idQuat                  operator-() const;
  63         idQuat &                operator=( const idQuat &a );
  64         idQuat                  operator+( const idQuat &a ) const;
  65         idQuat &                operator+=( const idQuat &a );
  66         idQuat                  operator-( const idQuat &a ) const;
  67         idQuat &                operator-=( const idQuat &a );
  68         idQuat                  operator*( const idQuat &a ) const;
  69         idVec3                  operator*( const idVec3 &a ) const;
  70         idQuat                  operator*( float a ) const;
  71         idQuat &                operator*=( const idQuat &a );
  72         idQuat &                operator*=( float a );
  73
  74         friend idQuat   operator*( const float a, const idQuat &b );
  75         friend idVec3   operator*( const idVec3 &a, const idQuat &b );
  76
  77         bool                    Compare( const idQuat &a ) const;                                               // exact compare, no epsilon
  78         bool                    Compare( const idQuat &a, const float epsilon ) const;  // compare with epsilon
  79         bool                    operator==(     const idQuat &a ) const;                                        // exact compare, no epsilon
  80         bool                    operator!=(     const idQuat &a ) const;                                        // exact compare, no epsilon
  81
  82         idQuat                  Inverse( void ) const;
  83         float                   Length( void ) const;
  84         idQuat &                Normalize( void );
  85
  86         float                   CalcW( void ) const;
  87         int                             GetDimension( void ) const;
  88
  89         idAngles                ToAngles( void ) const;
  90         idRotation              ToRotation( void ) const;
  91         idMat3                  ToMat3( void ) const;
  92         idMat4                  ToMat4( void ) const;
  93         idCQuat                 ToCQuat( void ) const;
  94         idVec3                  ToAngularVelocity( void ) const;
  95         const float *   ToFloatPtr( void ) const;
  96         float *                 ToFloatPtr( void );
  97         const char *    ToString( int precision = 2 ) const;
  98
  99         idQuat &                Slerp( const idQuat &from, const idQuat &to, float t );
 100 };
 101
 102 ID_INLINE idQuat::idQuat( void ) {
 103 }
 104
 105 ID_INLINE idQuat::idQuat( float x, float y, float z, float w ) {
 106         this->x = x;
 107         this->y = y;
 108         this->z = z;
 109         this->w = w;
 110 }
 111
 112 ID_INLINE float idQuat::operator[]( int index ) const {
 113         assert( ( index >= 0 ) && ( index < 4 ) );
 114         return ( &x )[ index ];
 115 }
 116
 117 ID_INLINE float& idQuat::operator[]( int index ) {
 118         assert( ( index >= 0 ) && ( index < 4 ) );
 119         return ( &x )[ index ];
 120 }
 121
 122 ID_INLINE idQuat idQuat::operator-() const {
 123         return idQuat( -x, -y, -z, -w );
 124 }
 125
 126 ID_INLINE idQuat &idQuat::operator=( const idQuat &a ) {
 127         x = a.x;
 128         y = a.y;
 129         z = a.z;
 130         w = a.w;
 131
 132         return *this;
 133 }
 134
 135 ID_INLINE idQuat idQuat::operator+( const idQuat &a ) const {
 136         return idQuat( x + a.x, y + a.y, z + a.z, w + a.w );
 137 }
 138
 139 ID_INLINE idQuat& idQuat::operator+=( const idQuat &a ) {
 140         x += a.x;
 141         y += a.y;
 142         z += a.z;
 143         w += a.w;
 144
 145         return *this;
 146 }
 147
 148 ID_INLINE idQuat idQuat::operator-( const idQuat &a ) const {
 149         return idQuat( x - a.x, y - a.y, z - a.z, w - a.w );
 150 }
 151
 152 ID_INLINE idQuat& idQuat::operator-=( const idQuat &a ) {
 153         x -= a.x;
 154         y -= a.y;
 155         z -= a.z;
 156         w -= a.w;
 157
 158         return *this;
 159 }
 160
 161 ID_INLINE idQuat idQuat::operator*( const idQuat &a ) const {
 162         return idQuat(  w*a.x + x*a.w + y*a.z - z*a.y,
 163                                         w*a.y + y*a.w + z*a.x - x*a.z,
 164                                         w*a.z + z*a.w + x*a.y - y*a.x,
 165                                         w*a.w - x*a.x - y*a.y - z*a.z );
 166 }
 167
 168 ID_INLINE idVec3 idQuat::operator*( const idVec3 &a ) const {
 169 #if 0
 170         // it's faster to do the conversion to a 3x3 matrix and multiply the vector by this 3x3 matrix
 171         return ( ToMat3() * a );
 172 #else
 173         // result = this->Inverse() * idQuat( a.x, a.y, a.z, 0.0f ) * (*this)
 174         float xxzz = x*x - z*z;
 175         float wwyy = w*w - y*y;
 176
 177         float xw2 = x*w*2.0f;
 178         float xy2 = x*y*2.0f;
 179         float xz2 = x*z*2.0f;
 180         float yw2 = y*w*2.0f;
 181         float yz2 = y*z*2.0f;
 182         float zw2 = z*w*2.0f;
 183
 184         return idVec3(
 185                 (xxzz + wwyy)*a.x               + (xy2 + zw2)*a.y               + (xz2 - yw2)*a.z,
 186                 (xy2 - zw2)*a.x                 + (y*y+w*w-x*x-z*z)*a.y + (yz2 + xw2)*a.z,
 187                 (xz2 + yw2)*a.x                 + (yz2 - xw2)*a.y               + (wwyy - xxzz)*a.z
 188         );
 189 #endif
 190 }
 191
 192 ID_INLINE idQuat idQuat::operator*( float a ) const {
 193         return idQuat( x * a, y * a, z * a, w * a );
 194 }
 195
 196 ID_INLINE idQuat operator*( const float a, const idQuat &b ) {
 197         return b * a;
 198 }
 199
 200 ID_INLINE idVec3 operator*( const idVec3 &a, const idQuat &b ) {
 201         return b * a;
 202 }
 203
 204 ID_INLINE idQuat& idQuat::operator*=( const idQuat &a ) {
 205         *this = *this * a;
 206
 207         return *this;
 208 }
 209
 210 ID_INLINE idQuat& idQuat::operator*=( float a ) {
 211         x *= a;
 212         y *= a;
 213         z *= a;
 214         w *= a;
 215
 216         return *this;
 217 }
 218
 219 ID_INLINE bool idQuat::Compare( const idQuat &a ) const {
 220         return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && ( w == a.w ) );
 221 }
 222
 223 ID_INLINE bool idQuat::Compare( const idQuat &a, const float epsilon ) const {
 224         if ( idMath::Fabs( x - a.x ) > epsilon ) {
 225                 return false;
 226         }
 227         if ( idMath::Fabs( y - a.y ) > epsilon ) {
 228                 return false;
 229         }
 230         if ( idMath::Fabs( z - a.z ) > epsilon ) {
 231                 return false;
 232         }
 233         if ( idMath::Fabs( w - a.w ) > epsilon ) {
 234                 return false;
 235         }
 236         return true;
 237 }
 238
 239 ID_INLINE bool idQuat::operator==( const idQuat &a ) const {
 240         return Compare( a );
 241 }
 242
 243 ID_INLINE bool idQuat::operator!=( const idQuat &a ) const {
 244         return !Compare( a );
 245 }
 246
 247 ID_INLINE void idQuat::Set( float x, float y, float z, float w ) {
 248         this->x = x;
 249         this->y = y;
 250         this->z = z;
 251         this->w = w;
 252 }
 253
 254 ID_INLINE idQuat idQuat::Inverse( void ) const {
 255         return idQuat( -x, -y, -z, w );
 256 }
 257
 258 ID_INLINE float idQuat::Length( void ) const {
 259         float len;
 260
 261         len = x * x + y * y + z * z + w * w;
 262         return idMath::Sqrt( len );
 263 }
 264
 265 ID_INLINE idQuat& idQuat::Normalize( void ) {
 266         float len;
 267         float ilength;
 268
 269         len = this->Length();
 270         if ( len ) {
 271                 ilength = 1 / len;
 272                 x *= ilength;
 273                 y *= ilength;
 274                 z *= ilength;
 275                 w *= ilength;
 276         }
 277         return *this;
 278 }
 279
 280 ID_INLINE float idQuat::CalcW( void ) const {
 281         // take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
 282         return sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) );
 283 }
 284
 285 ID_INLINE int idQuat::GetDimension( void ) const {
 286         return 4;
 287 }
 288
 289 ID_INLINE const float *idQuat::ToFloatPtr( void ) const {
 290         return &x;
 291 }
 292
 293 ID_INLINE float *idQuat::ToFloatPtr( void ) {
 294         return &x;
 295 }
 296
 297
 298 /*
 299 ===============================================================================
 300
 301         Compressed quaternion
 302
 303 ===============================================================================
 304 */
 305
 306 class idCQuat {
 307 public:
 308         float                   x;
 309         float                   y;
 310         float                   z;
 311
 312                                         idCQuat( void );
 313                                         idCQuat( float x, float y, float z );
 314
 315         void                    Set( float x, float y, float z );
 316
 317         float                   operator[]( int index ) const;
 318         float &                 operator[]( int index );
 319
 320         bool                    Compare( const idCQuat &a ) const;                                              // exact compare, no epsilon
 321         bool                    Compare( const idCQuat &a, const float epsilon ) const; // compare with epsilon
 322         bool                    operator==(     const idCQuat &a ) const;                                       // exact compare, no epsilon
 323         bool                    operator!=(     const idCQuat &a ) const;                                       // exact compare, no epsilon
 324
 325         int                             GetDimension( void ) const;
 326
 327         idAngles                ToAngles( void ) const;
 328         idRotation              ToRotation( void ) const;
 329         idMat3                  ToMat3( void ) const;
 330         idMat4                  ToMat4( void ) const;
 331         idQuat                  ToQuat( void ) const;
 332         const float *   ToFloatPtr( void ) const;
 333         float *                 ToFloatPtr( void );
 334         const char *    ToString( int precision = 2 ) const;
 335 };
 336
 337 ID_INLINE idCQuat::idCQuat( void ) {
 338 }
 339
 340 ID_INLINE idCQuat::idCQuat( float x, float y, float z ) {
 341         this->x = x;
 342         this->y = y;
 343         this->z = z;
 344 }
 345
 346 ID_INLINE void idCQuat::Set( float x, float y, float z ) {
 347         this->x = x;
 348         this->y = y;
 349         this->z = z;
 350 }
 351
 352 ID_INLINE float idCQuat::operator[]( int index ) const {
 353         assert( ( index >= 0 ) && ( index < 3 ) );
 354         return ( &x )[ index ];
 355 }
 356
 357 ID_INLINE float& idCQuat::operator[]( int index ) {
 358         assert( ( index >= 0 ) && ( index < 3 ) );
 359         return ( &x )[ index ];
 360 }
 361
 362 ID_INLINE bool idCQuat::Compare( const idCQuat &a ) const {
 363         return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) );
 364 }
 365
 366 ID_INLINE bool idCQuat::Compare( const idCQuat &a, const float epsilon ) const {
 367         if ( idMath::Fabs( x - a.x ) > epsilon ) {
 368                 return false;
 369         }
 370         if ( idMath::Fabs( y - a.y ) > epsilon ) {
 371                 return false;
 372         }
 373         if ( idMath::Fabs( z - a.z ) > epsilon ) {
 374                 return false;
 375         }
 376         return true;
 377 }
 378
 379 ID_INLINE bool idCQuat::operator==( const idCQuat &a ) const {
 380         return Compare( a );
 381 }
 382
 383 ID_INLINE bool idCQuat::operator!=( const idCQuat &a ) const {
 384         return !Compare( a );
 385 }
 386
 387 ID_INLINE int idCQuat::GetDimension( void ) const {
 388         return 3;
 389 }
 390
 391 ID_INLINE idQuat idCQuat::ToQuat( void ) const {
 392         // take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
 393         return idQuat( x, y, z, sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) ) );
 394 }
 395
 396 ID_INLINE const float *idCQuat::ToFloatPtr( void ) const {
 397         return &x;
 398 }
 399
 400 ID_INLINE float *idCQuat::ToFloatPtr( void ) {
 401         return &x;
 402 }
 403
 404 #endif /* !__MATH_QUAT_H__ */