DOOM-3-BFG

DOOM 3 BFG Edition
Log | Files | Refs

Quat.h (11114B)

      1 /*
      2 ===========================================================================
      3 
      4 Doom 3 BFG Edition GPL Source Code
      5 Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company. 
      6 
      7 This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").  
      8 
      9 Doom 3 BFG Edition 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 BFG Edition 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 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.
     21 
     22 In addition, the Doom 3 BFG Edition 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 BFG Edition 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 #include "../containers/Array.h"	// for idTupleSize
     41 
     42 class idVec3;
     43 class idAngles;
     44 class idRotation;
     45 class idMat3;
     46 class idMat4;
     47 class idCQuat;
     48 
     49 class idQuat {
     50 public:
     51 	float			x;
     52 	float			y;
     53 	float			z;
     54 	float			w;
     55 
     56 					idQuat();
     57 					idQuat( float x, float y, float z, float w );
     58 
     59 	void 			Set( float x, float y, float z, float w );
     60 
     61 	float			operator[]( int index ) const;
     62 	float &			operator[]( int index );
     63 	idQuat			operator-() const;
     64 	idQuat &		operator=( const idQuat &a );
     65 	idQuat			operator+( const idQuat &a ) const;
     66 	idQuat &		operator+=( const idQuat &a );
     67 	idQuat			operator-( const idQuat &a ) const;
     68 	idQuat &		operator-=( const idQuat &a );
     69 	idQuat			operator*( const idQuat &a ) const;
     70 	idVec3			operator*( const idVec3 &a ) const;
     71 	idQuat			operator*( float a ) const;
     72 	idQuat &		operator*=( const idQuat &a );
     73 	idQuat &		operator*=( float a );
     74 
     75 	friend idQuat	operator*( const float a, const idQuat &b );
     76 	friend idVec3	operator*( const idVec3 &a, const idQuat &b );
     77 
     78 	bool			Compare( const idQuat &a ) const;						// exact compare, no epsilon
     79 	bool			Compare( const idQuat &a, const float epsilon ) const;	// compare with epsilon
     80 	bool			operator==(	const idQuat &a ) const;					// exact compare, no epsilon
     81 	bool			operator!=(	const idQuat &a ) const;					// exact compare, no epsilon
     82 
     83 	idQuat			Inverse() const;
     84 	float			Length() const;
     85 	idQuat &		Normalize();
     86 
     87 	float			CalcW() const;
     88 	int				GetDimension() const;
     89 
     90 	idAngles		ToAngles() const;
     91 	idRotation		ToRotation() const;
     92 	idMat3			ToMat3() const;
     93 	idMat4			ToMat4() const;
     94 	idCQuat			ToCQuat() const;
     95 	idVec3			ToAngularVelocity() const;
     96 	const float *	ToFloatPtr() const;
     97 	float *			ToFloatPtr();
     98 	const char *	ToString( int precision = 2 ) const;
     99 
    100 	idQuat &		Slerp( const idQuat &from, const idQuat &to, float t );
    101 	idQuat &		Lerp( const idQuat &from, const idQuat &to, const float t );
    102 };
    103 
    104 // A non-member slerp function allows constructing a const idQuat object with the result of a slerp,
    105 // but without having to explicity create a temporary idQuat object.
    106 idQuat Slerp( const idQuat & from, const idQuat & to, const float t );
    107 
    108 ID_INLINE idQuat::idQuat() {
    109 }
    110 
    111 ID_INLINE idQuat::idQuat( float x, float y, float z, float w ) {
    112 	this->x = x;
    113 	this->y = y;
    114 	this->z = z;
    115 	this->w = w;
    116 }
    117 
    118 ID_INLINE float idQuat::operator[]( int index ) const {
    119 	assert( ( index >= 0 ) && ( index < 4 ) );
    120 	return ( &x )[ index ];
    121 }
    122 
    123 ID_INLINE float& idQuat::operator[]( int index ) {
    124 	assert( ( index >= 0 ) && ( index < 4 ) );
    125 	return ( &x )[ index ];
    126 }
    127 
    128 ID_INLINE idQuat idQuat::operator-() const {
    129 	return idQuat( -x, -y, -z, -w );
    130 }
    131 
    132 ID_INLINE idQuat &idQuat::operator=( const idQuat &a ) {
    133 	x = a.x;
    134 	y = a.y;
    135 	z = a.z;
    136 	w = a.w;
    137 
    138 	return *this;
    139 }
    140 
    141 ID_INLINE idQuat idQuat::operator+( const idQuat &a ) const {
    142 	return idQuat( x + a.x, y + a.y, z + a.z, w + a.w );
    143 }
    144 
    145 ID_INLINE idQuat& idQuat::operator+=( const idQuat &a ) {
    146 	x += a.x;
    147 	y += a.y;
    148 	z += a.z;
    149 	w += a.w;
    150 
    151 	return *this;
    152 }
    153 
    154 ID_INLINE idQuat idQuat::operator-( const idQuat &a ) const {
    155 	return idQuat( x - a.x, y - a.y, z - a.z, w - a.w );
    156 }
    157 
    158 ID_INLINE idQuat& idQuat::operator-=( const idQuat &a ) {
    159 	x -= a.x;
    160 	y -= a.y;
    161 	z -= a.z;
    162 	w -= a.w;
    163 
    164 	return *this;
    165 }
    166 
    167 ID_INLINE idQuat idQuat::operator*( const idQuat &a ) const {
    168 	return idQuat(	w*a.x + x*a.w + y*a.z - z*a.y,
    169 					w*a.y + y*a.w + z*a.x - x*a.z,
    170 					w*a.z + z*a.w + x*a.y - y*a.x,
    171 					w*a.w - x*a.x - y*a.y - z*a.z );
    172 }
    173 
    174 ID_INLINE idVec3 idQuat::operator*( const idVec3 &a ) const {
    175 #if 0
    176 	// it's faster to do the conversion to a 3x3 matrix and multiply the vector by this 3x3 matrix
    177 	return ( ToMat3() * a );
    178 #else
    179 	// result = this->Inverse() * idQuat( a.x, a.y, a.z, 0.0f ) * (*this)
    180 	float xxzz = x*x - z*z;
    181 	float wwyy = w*w - y*y;
    182 
    183 	float xw2 = x*w*2.0f;
    184 	float xy2 = x*y*2.0f;
    185 	float xz2 = x*z*2.0f;
    186 	float yw2 = y*w*2.0f;
    187 	float yz2 = y*z*2.0f;
    188 	float zw2 = z*w*2.0f;
    189 
    190 	return idVec3(
    191 		(xxzz + wwyy)*a.x		+ (xy2 + zw2)*a.y		+ (xz2 - yw2)*a.z,
    192 		(xy2 - zw2)*a.x			+ (y*y+w*w-x*x-z*z)*a.y	+ (yz2 + xw2)*a.z,
    193 		(xz2 + yw2)*a.x			+ (yz2 - xw2)*a.y		+ (wwyy - xxzz)*a.z
    194 	);
    195 #endif
    196 }
    197 
    198 ID_INLINE idQuat idQuat::operator*( float a ) const {
    199 	return idQuat( x * a, y * a, z * a, w * a );
    200 }
    201 
    202 ID_INLINE idQuat operator*( const float a, const idQuat &b ) {
    203 	return b * a;
    204 }
    205 
    206 ID_INLINE idVec3 operator*( const idVec3 &a, const idQuat &b ) {
    207 	return b * a;
    208 }
    209 
    210 ID_INLINE idQuat& idQuat::operator*=( const idQuat &a ) {
    211 	*this = *this * a;
    212 
    213 	return *this;
    214 }
    215 
    216 ID_INLINE idQuat& idQuat::operator*=( float a ) {
    217 	x *= a;
    218 	y *= a;
    219 	z *= a;
    220 	w *= a;
    221 
    222 	return *this;
    223 }
    224 
    225 ID_INLINE bool idQuat::Compare( const idQuat &a ) const {
    226 	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) && ( w == a.w ) );
    227 }
    228 
    229 ID_INLINE bool idQuat::Compare( const idQuat &a, const float epsilon ) const {
    230 	if ( idMath::Fabs( x - a.x ) > epsilon ) {
    231 		return false;
    232 	}
    233 	if ( idMath::Fabs( y - a.y ) > epsilon ) {
    234 		return false;
    235 	}
    236 	if ( idMath::Fabs( z - a.z ) > epsilon ) {
    237 		return false;
    238 	}
    239 	if ( idMath::Fabs( w - a.w ) > epsilon ) {
    240 		return false;
    241 	}
    242 	return true;
    243 }
    244 
    245 ID_INLINE bool idQuat::operator==( const idQuat &a ) const {
    246 	return Compare( a );
    247 }
    248 
    249 ID_INLINE bool idQuat::operator!=( const idQuat &a ) const {
    250 	return !Compare( a );
    251 }
    252 
    253 ID_INLINE void idQuat::Set( float x, float y, float z, float w ) {
    254 	this->x = x;
    255 	this->y = y;
    256 	this->z = z;
    257 	this->w = w;
    258 }
    259 
    260 ID_INLINE idQuat idQuat::Inverse() const {
    261 	return idQuat( -x, -y, -z, w );
    262 }
    263 
    264 ID_INLINE float idQuat::Length() const {
    265 	float len;
    266 
    267 	len = x * x + y * y + z * z + w * w;
    268 	return idMath::Sqrt( len );
    269 }
    270 
    271 ID_INLINE idQuat& idQuat::Normalize() {
    272 	float len;
    273 	float ilength;
    274 
    275 	len = this->Length();
    276 	if ( len ) {
    277 		ilength = 1 / len;
    278 		x *= ilength;
    279 		y *= ilength;
    280 		z *= ilength;
    281 		w *= ilength;
    282 	}
    283 	return *this;
    284 }
    285 
    286 ID_INLINE float idQuat::CalcW() const {
    287 	// take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
    288 	return sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) );
    289 }
    290 
    291 ID_INLINE int idQuat::GetDimension() const {
    292 	return 4;
    293 }
    294 
    295 ID_INLINE const float *idQuat::ToFloatPtr() const {
    296 	return &x;
    297 }
    298 
    299 ID_INLINE float *idQuat::ToFloatPtr() {
    300 	return &x;
    301 }
    302 
    303 /*
    304 ===============================================================================
    305 
    306 	Specialization to get size of an idQuat generically.
    307 
    308 ===============================================================================
    309 */
    310 template<>
    311 struct idTupleSize< idQuat > {
    312 	enum { value = 4 };
    313 };
    314 
    315 /*
    316 ===============================================================================
    317 
    318 	Compressed quaternion
    319 
    320 ===============================================================================
    321 */
    322 
    323 class idCQuat {
    324 public:
    325 	float			x;
    326 	float			y;
    327 	float			z;
    328 
    329 					idCQuat();
    330 					idCQuat( float x, float y, float z );
    331 
    332 	void 			Set( float x, float y, float z );
    333 
    334 	float			operator[]( int index ) const;
    335 	float &			operator[]( int index );
    336 
    337 	bool			Compare( const idCQuat &a ) const;						// exact compare, no epsilon
    338 	bool			Compare( const idCQuat &a, const float epsilon ) const;	// compare with epsilon
    339 	bool			operator==(	const idCQuat &a ) const;					// exact compare, no epsilon
    340 	bool			operator!=(	const idCQuat &a ) const;					// exact compare, no epsilon
    341 
    342 	int				GetDimension() const;
    343 
    344 	idAngles		ToAngles() const;
    345 	idRotation		ToRotation() const;
    346 	idMat3			ToMat3() const;
    347 	idMat4			ToMat4() const;
    348 	idQuat			ToQuat() const;
    349 	const float *	ToFloatPtr() const;
    350 	float *			ToFloatPtr();
    351 	const char *	ToString( int precision = 2 ) const;
    352 };
    353 
    354 ID_INLINE idCQuat::idCQuat() {
    355 }
    356 
    357 ID_INLINE idCQuat::idCQuat( float x, float y, float z ) {
    358 	this->x = x;
    359 	this->y = y;
    360 	this->z = z;
    361 }
    362 
    363 ID_INLINE void idCQuat::Set( float x, float y, float z ) {
    364 	this->x = x;
    365 	this->y = y;
    366 	this->z = z;
    367 }
    368 
    369 ID_INLINE float idCQuat::operator[]( int index ) const {
    370 	assert( ( index >= 0 ) && ( index < 3 ) );
    371 	return ( &x )[ index ];
    372 }
    373 
    374 ID_INLINE float& idCQuat::operator[]( int index ) {
    375 	assert( ( index >= 0 ) && ( index < 3 ) );
    376 	return ( &x )[ index ];
    377 }
    378 
    379 ID_INLINE bool idCQuat::Compare( const idCQuat &a ) const {
    380 	return ( ( x == a.x ) && ( y == a.y ) && ( z == a.z ) );
    381 }
    382 
    383 ID_INLINE bool idCQuat::Compare( const idCQuat &a, const float epsilon ) const {
    384 	if ( idMath::Fabs( x - a.x ) > epsilon ) {
    385 		return false;
    386 	}
    387 	if ( idMath::Fabs( y - a.y ) > epsilon ) {
    388 		return false;
    389 	}
    390 	if ( idMath::Fabs( z - a.z ) > epsilon ) {
    391 		return false;
    392 	}
    393 	return true;
    394 }
    395 
    396 ID_INLINE bool idCQuat::operator==( const idCQuat &a ) const {
    397 	return Compare( a );
    398 }
    399 
    400 ID_INLINE bool idCQuat::operator!=( const idCQuat &a ) const {
    401 	return !Compare( a );
    402 }
    403 
    404 ID_INLINE int idCQuat::GetDimension() const {
    405 	return 3;
    406 }
    407 
    408 ID_INLINE idQuat idCQuat::ToQuat() const {
    409 	// take the absolute value because floating point rounding may cause the dot of x,y,z to be larger than 1
    410 	return idQuat( x, y, z, sqrt( fabs( 1.0f - ( x * x + y * y + z * z ) ) ) );
    411 }
    412 
    413 ID_INLINE const float *idCQuat::ToFloatPtr() const {
    414 	return &x;
    415 }
    416 
    417 ID_INLINE float *idCQuat::ToFloatPtr() {
    418 	return &x;
    419 }
    420 
    421 /*
    422 ===============================================================================
    423 
    424 	Specialization to get size of an idCQuat generically.
    425 
    426 ===============================================================================
    427 */
    428 template<>
    429 struct idTupleSize< idCQuat > {
    430 	enum { value = 3 };
    431 };
    432 
    433 #endif /* !__MATH_QUAT_H__ */