DOOM-3-BFG

DOOM 3 BFG Edition
Log | Files | Refs

VecX.h (19057B)

      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_VECX_H__
     30 #define __MATH_VECX_H__
     31 
     32 /*
     33 ===============================================================================
     34 
     35 idVecX - arbitrary sized vector
     36 
     37 The vector lives on 16 byte aligned and 16 byte padded memory.
     38 
     39 NOTE: due to the temporary memory pool idVecX cannot be used by multiple threads
     40 
     41 ===============================================================================
     42 */
     43 
     44 #define VECX_MAX_TEMP		1024
     45 #define VECX_QUAD( x )		( ( ( ( x ) + 3 ) & ~3 ) * sizeof( float ) )
     46 #define VECX_CLEAREND()		int s = size; while( s < ( ( s + 3) & ~3 ) ) { p[s++] = 0.0f; }
     47 #define VECX_ALLOCA( n )	( (float *) _alloca16( VECX_QUAD( n ) ) )
     48 #define VECX_SIMD
     49 
     50 class idVecX {
     51 	friend class idMatX;
     52 
     53 public:	
     54 	ID_INLINE					idVecX();
     55 	ID_INLINE					explicit idVecX( int length );
     56 	ID_INLINE					explicit idVecX( int length, float *data );
     57 	ID_INLINE					~idVecX();
     58 
     59 	ID_INLINE	float			Get( int index ) const;
     60 	ID_INLINE	float &			Get( int index );
     61 
     62 	ID_INLINE	float			operator[]( const int index ) const;
     63 	ID_INLINE	float &			operator[]( const int index );
     64 	ID_INLINE	idVecX			operator-() const;
     65 	ID_INLINE	idVecX &		operator=( const idVecX &a );
     66 	ID_INLINE	idVecX			operator*( const float a ) const;
     67 	ID_INLINE	idVecX			operator/( const float a ) const;
     68 	ID_INLINE	float			operator*( const idVecX &a ) const;
     69 	ID_INLINE	idVecX			operator-( const idVecX &a ) const;
     70 	ID_INLINE	idVecX			operator+( const idVecX &a ) const;
     71 	ID_INLINE	idVecX &		operator*=( const float a );
     72 	ID_INLINE	idVecX &		operator/=( const float a );
     73 	ID_INLINE	idVecX &		operator+=( const idVecX &a );
     74 	ID_INLINE	idVecX &		operator-=( const idVecX &a );
     75 
     76 	friend ID_INLINE	idVecX	operator*( const float a, const idVecX &b );
     77 
     78 	ID_INLINE	bool			Compare( const idVecX &a ) const;							// exact compare, no epsilon
     79 	ID_INLINE	bool			Compare( const idVecX &a, const float epsilon ) const;		// compare with epsilon
     80 	ID_INLINE	bool			operator==(	const idVecX &a ) const;						// exact compare, no epsilon
     81 	ID_INLINE	bool			operator!=(	const idVecX &a ) const;						// exact compare, no epsilon
     82 
     83 	ID_INLINE	void			SetSize( int size );
     84 	ID_INLINE	void			ChangeSize( int size, bool makeZero = false );
     85 	ID_INLINE	int				GetSize() const { return size; }
     86 	ID_INLINE	void			SetData( int length, float *data );
     87 	ID_INLINE	void			Zero();
     88 	ID_INLINE	void			Zero( int length );
     89 	ID_INLINE	void			Random( int seed, float l = 0.0f, float u = 1.0f );
     90 	ID_INLINE	void			Random( int length, int seed, float l = 0.0f, float u = 1.0f );
     91 	ID_INLINE	void			Negate();
     92 	ID_INLINE	void			Clamp( float min, float max );
     93 	ID_INLINE	idVecX &		SwapElements( int e1, int e2 );
     94 
     95 	ID_INLINE	float			Length() const;
     96 	ID_INLINE	float			LengthSqr() const;
     97 	ID_INLINE	idVecX			Normalize() const;
     98 	ID_INLINE	float			NormalizeSelf();
     99 
    100 	ID_INLINE	int				GetDimension() const;
    101 
    102 	ID_INLINE	void			AddScaleAdd( const float scale, const idVecX & v0, const idVecX & v1 );
    103 
    104 	ID_INLINE	const idVec3 &	SubVec3( int index ) const;
    105 	ID_INLINE	idVec3 &		SubVec3( int index );
    106 	ID_INLINE	const idVec6 &	SubVec6( int index = 0 ) const;
    107 	ID_INLINE	idVec6 &		SubVec6( int index = 0 );
    108 	ID_INLINE	const float *	ToFloatPtr() const;
    109 	ID_INLINE	float *			ToFloatPtr();
    110 	const char *	ToString( int precision = 2 ) const;
    111 
    112 private:
    113 	int				size;					// size of the vector
    114 	int				alloced;				// if -1 p points to data set with SetData
    115 	float *			p;						// memory the vector is stored
    116 
    117 	static float	temp[VECX_MAX_TEMP+4];	// used to store intermediate results
    118 	static float *	tempPtr;				// pointer to 16 byte aligned temporary memory
    119 	static int		tempIndex;				// index into memory pool, wraps around
    120 
    121 	ID_INLINE void	SetTempSize( int size );
    122 };
    123 
    124 
    125 /*
    126 ========================
    127 idVecX::idVecX
    128 ========================
    129 */
    130 ID_INLINE idVecX::idVecX() {
    131 	size = alloced = 0;
    132 	p = NULL;
    133 }
    134 
    135 /*
    136 ========================
    137 idVecX::idVecX
    138 ========================
    139 */
    140 ID_INLINE idVecX::idVecX( int length ) {
    141 	size = alloced = 0;
    142 	p = NULL;
    143 	SetSize( length );
    144 }
    145 
    146 /*
    147 ========================
    148 idVecX::idVecX
    149 ========================
    150 */
    151 ID_INLINE idVecX::idVecX( int length, float *data ) {
    152 	size = alloced = 0;
    153 	p = NULL;
    154 	SetData( length, data );
    155 }
    156 
    157 /*
    158 ========================
    159 idVecX::~idVecX
    160 ========================
    161 */
    162 ID_INLINE idVecX::~idVecX() {
    163 	// if not temp memory
    164 	if ( p && ( p < idVecX::tempPtr || p >= idVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
    165 		Mem_Free16( p );
    166 	}
    167 }
    168 
    169 /*
    170 ========================
    171 idVecX::Get
    172 ========================
    173 */
    174 ID_INLINE float idVecX::Get( int index ) const {
    175 	assert( index >= 0 && index < size );
    176 	return p[index];
    177 }
    178 
    179 /*
    180 ========================
    181 idVecX::Get
    182 ========================
    183 */
    184 ID_INLINE float & idVecX::Get( int index ) {
    185 	assert( index >= 0 && index < size );
    186 	return p[index];
    187 }
    188 
    189 /*
    190 ========================
    191 idVecX::operator[]
    192 ========================
    193 */
    194 ID_INLINE float idVecX::operator[]( int index ) const {
    195 	return Get( index );
    196 }
    197 
    198 /*
    199 ========================
    200 idVecX::operator[]
    201 ========================
    202 */
    203 ID_INLINE float & idVecX::operator[]( int index ) {
    204 	return Get( index );
    205 }
    206 
    207 /*
    208 ========================
    209 idVecX::operator-
    210 ========================
    211 */
    212 ID_INLINE idVecX idVecX::operator-() const {
    213 	idVecX m;
    214 
    215 	m.SetTempSize( size );
    216 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    217 	ALIGN16( unsigned int signBit[4] ) = { IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK };
    218 	for ( int i = 0; i < size; i += 4 ) {
    219 		_mm_store_ps( m.p + i, _mm_xor_ps( _mm_load_ps( p + i ), (__m128 &) signBit[0] ) );
    220 	}
    221 #else
    222 	for ( int i = 0; i < size; i++ ) {
    223 		m.p[i] = -p[i];
    224 	}
    225 #endif
    226 	return m;
    227 }
    228 
    229 /*
    230 ========================
    231 idVecX::operator=
    232 ========================
    233 */
    234 ID_INLINE idVecX &idVecX::operator=( const idVecX &a ) { 
    235 	SetSize( a.size );
    236 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    237 	for ( int i = 0; i < a.size; i += 4 ) {
    238 		_mm_store_ps( p + i, _mm_load_ps( a.p + i ) );
    239 	}
    240 #else
    241 	memcpy( p, a.p, a.size * sizeof( float ) );
    242 #endif
    243 	idVecX::tempIndex = 0;
    244 	return *this;
    245 }
    246 
    247 /*
    248 ========================
    249 idVecX::operator+
    250 ========================
    251 */
    252 ID_INLINE idVecX idVecX::operator+( const idVecX &a ) const {
    253 	idVecX m;
    254 
    255 	assert( size == a.size );
    256 	m.SetTempSize( size );
    257 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    258 	for ( int i = 0; i < size; i += 4 ) {
    259 		_mm_store_ps( m.p + i, _mm_add_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
    260 	}
    261 #else
    262 	for ( int i = 0; i < size; i++ ) {
    263 		m.p[i] = p[i] + a.p[i];
    264 	}
    265 #endif
    266 	return m;
    267 }
    268 
    269 /*
    270 ========================
    271 idVecX::operator-
    272 ========================
    273 */
    274 ID_INLINE idVecX idVecX::operator-( const idVecX &a ) const {
    275 	idVecX m;
    276 
    277 	assert( size == a.size );
    278 	m.SetTempSize( size );
    279 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    280 	for ( int i = 0; i < size; i += 4 ) {
    281 		_mm_store_ps( m.p + i, _mm_sub_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
    282 	}
    283 #else
    284 	for ( int i = 0; i < size; i++ ) {
    285 		m.p[i] = p[i] - a.p[i];
    286 	}
    287 #endif
    288 	return m;
    289 }
    290 
    291 /*
    292 ========================
    293 idVecX::operator+=
    294 ========================
    295 */
    296 ID_INLINE idVecX &idVecX::operator+=( const idVecX &a ) {
    297 	assert( size == a.size );
    298 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    299 	for ( int i = 0; i < size; i += 4 ) {
    300 		_mm_store_ps( p + i, _mm_add_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
    301 	}
    302 #else
    303 	for ( int i = 0; i < size; i++ ) {
    304 		p[i] += a.p[i];
    305 	}
    306 #endif
    307 	idVecX::tempIndex = 0;
    308 	return *this;
    309 }
    310 
    311 /*
    312 ========================
    313 idVecX::operator-=
    314 ========================
    315 */
    316 ID_INLINE idVecX &idVecX::operator-=( const idVecX &a ) {
    317 	assert( size == a.size );
    318 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    319 	for ( int i = 0; i < size; i += 4 ) {
    320 		_mm_store_ps( p + i, _mm_sub_ps( _mm_load_ps( p + i ), _mm_load_ps( a.p + i ) ) );
    321 	}
    322 #else
    323 	for ( int i = 0; i < size; i++ ) {
    324 		p[i] -= a.p[i];
    325 	}
    326 #endif
    327 	idVecX::tempIndex = 0;
    328 	return *this;
    329 }
    330 
    331 /*
    332 ========================
    333 idVecX::operator*
    334 ========================
    335 */
    336 ID_INLINE idVecX idVecX::operator*( const float a ) const {
    337 	idVecX m;
    338 
    339 	m.SetTempSize( size );
    340 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    341 	__m128 va = _mm_load1_ps( & a );
    342 	for ( int i = 0; i < size; i += 4 ) {
    343 		_mm_store_ps( m.p + i, _mm_mul_ps( _mm_load_ps( p + i ), va ) );
    344 	}
    345 #else
    346 	for ( int i = 0; i < size; i++ ) {
    347 		m.p[i] = p[i] * a;
    348 	}
    349 #endif
    350 	return m;
    351 }
    352 
    353 /*
    354 ========================
    355 idVecX::operator*=
    356 ========================
    357 */
    358 ID_INLINE idVecX &idVecX::operator*=( const float a ) {
    359 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    360 	__m128 va = _mm_load1_ps( & a );
    361 	for ( int i = 0; i < size; i += 4 ) {
    362 		_mm_store_ps( p + i, _mm_mul_ps( _mm_load_ps( p + i ), va ) );
    363 	}
    364 #else
    365 	for ( int i = 0; i < size; i++ ) {
    366 		p[i] *= a;
    367 	}
    368 #endif
    369 	return *this;
    370 }
    371 
    372 /*
    373 ========================
    374 idVecX::operator/
    375 ========================
    376 */
    377 ID_INLINE idVecX idVecX::operator/( const float a ) const {
    378 	assert( fabs( a ) > idMath::FLT_SMALLEST_NON_DENORMAL );
    379 	return (*this) * ( 1.0f / a );
    380 }
    381 
    382 /*
    383 ========================
    384 idVecX::operator/=
    385 ========================
    386 */
    387 ID_INLINE idVecX &idVecX::operator/=( const float a ) {
    388 	assert( fabs( a ) > idMath::FLT_SMALLEST_NON_DENORMAL );
    389 	(*this) *= ( 1.0f / a );
    390 	return *this;
    391 }
    392 
    393 /*
    394 ========================
    395 operator*
    396 ========================
    397 */
    398 ID_INLINE idVecX operator*( const float a, const idVecX &b ) {
    399 	return b * a;
    400 }
    401 
    402 /*
    403 ========================
    404 idVecX::operator*
    405 ========================
    406 */
    407 ID_INLINE float idVecX::operator*( const idVecX &a ) const {
    408 	assert( size == a.size );
    409 	float sum = 0.0f;
    410 	for ( int i = 0; i < size; i++ ) {
    411 		sum += p[i] * a.p[i];
    412 	}
    413 	return sum;
    414 }
    415 
    416 /*
    417 ========================
    418 idVecX::Compare
    419 ========================
    420 */
    421 ID_INLINE bool idVecX::Compare( const idVecX &a ) const {
    422 	assert( size == a.size );
    423 	for ( int i = 0; i < size; i++ ) {
    424 		if ( p[i] != a.p[i] ) {
    425 			return false;
    426 		}
    427 	}
    428 	return true;
    429 }
    430 
    431 /*
    432 ========================
    433 idVecX::Compare
    434 ========================
    435 */
    436 ID_INLINE bool idVecX::Compare( const idVecX &a, const float epsilon ) const {
    437 	assert( size == a.size );
    438 	for ( int i = 0; i < size; i++ ) {
    439 		if ( idMath::Fabs( p[i] - a.p[i] ) > epsilon ) {
    440 			return false;
    441 		}
    442 	}
    443 	return true;
    444 }
    445 
    446 /*
    447 ========================
    448 idVecX::operator==
    449 ========================
    450 */
    451 ID_INLINE bool idVecX::operator==( const idVecX &a ) const {
    452 	return Compare( a );
    453 }
    454 
    455 /*
    456 ========================
    457 idVecX::operator!=
    458 ========================
    459 */
    460 ID_INLINE bool idVecX::operator!=( const idVecX &a ) const {
    461 	return !Compare( a );
    462 }
    463 
    464 /*
    465 ========================
    466 idVecX::SetSize
    467 ========================
    468 */
    469 ID_INLINE void idVecX::SetSize( int newSize ) {
    470 	//assert( p < idVecX::tempPtr || p > idVecX::tempPtr + VECX_MAX_TEMP );
    471 	if ( newSize != size || p == NULL ) {
    472 		int alloc = ( newSize + 3 ) & ~3;
    473 		if ( alloc > alloced && alloced != -1 ) {
    474 			if ( p ) {
    475 				Mem_Free16( p );
    476 			}
    477 			p = (float *) Mem_Alloc16( alloc * sizeof( float ), TAG_MATH );
    478 			alloced = alloc;
    479 		}
    480 		size = newSize;
    481 		VECX_CLEAREND();
    482 	}
    483 }
    484 
    485 /*
    486 ========================
    487 idVecX::ChangeSize
    488 ========================
    489 */
    490 ID_INLINE void idVecX::ChangeSize( int newSize, bool makeZero ) {
    491 	if ( newSize != size ) {
    492 		int alloc = ( newSize + 3 ) & ~3;
    493 		if ( alloc > alloced && alloced != -1 ) {
    494 			float *oldVec = p;
    495 			p = (float *) Mem_Alloc16( alloc * sizeof( float ), TAG_MATH );
    496 			alloced = alloc;
    497 			if ( oldVec ) {
    498 				for ( int i = 0; i < size; i++ ) {
    499 					p[i] = oldVec[i];
    500 				}
    501 				Mem_Free16( oldVec );
    502 			}
    503 			if ( makeZero ) {
    504 				// zero any new elements
    505 				for ( int i = size; i < newSize; i++ ) {
    506 					p[i] = 0.0f;
    507 				}
    508 			}
    509 		}
    510 		size = newSize;
    511 		VECX_CLEAREND();
    512 	}
    513 }
    514 
    515 /*
    516 ========================
    517 idVecX::SetTempSize
    518 ========================
    519 */
    520 ID_INLINE void idVecX::SetTempSize( int newSize ) {
    521 	size = newSize;
    522 	alloced = ( newSize + 3 ) & ~3;
    523 	assert( alloced < VECX_MAX_TEMP );
    524 	if ( idVecX::tempIndex + alloced > VECX_MAX_TEMP ) {
    525 		idVecX::tempIndex = 0;
    526 	}
    527 	p = idVecX::tempPtr + idVecX::tempIndex;
    528 	idVecX::tempIndex += alloced;
    529 	VECX_CLEAREND();
    530 }
    531 
    532 /*
    533 ========================
    534 idVecX::SetData
    535 ========================
    536 */
    537 ID_INLINE void idVecX::SetData( int length, float *data ) {
    538 	if ( p != NULL && ( p < idVecX::tempPtr || p >= idVecX::tempPtr + VECX_MAX_TEMP ) && alloced != -1 ) {
    539 		Mem_Free16( p );
    540 	}
    541 	assert_16_byte_aligned( data ); // data must be 16 byte aligned
    542 	p = data;
    543 	size = length;
    544 	alloced = -1;
    545 	VECX_CLEAREND();
    546 }
    547 
    548 /*
    549 ========================
    550 idVecX::Zero
    551 ========================
    552 */
    553 ID_INLINE void idVecX::Zero() {
    554 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    555 	for ( int i = 0; i < size; i += 4 ) {
    556 		_mm_store_ps( p + i, _mm_setzero_ps() );
    557 	}
    558 #else
    559 	memset( p, 0, size * sizeof( float ) );
    560 #endif
    561 }
    562 
    563 /*
    564 ========================
    565 idVecX::Zero
    566 ========================
    567 */
    568 ID_INLINE void idVecX::Zero( int length ) {
    569 	SetSize( length );
    570 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    571 	for ( int i = 0; i < length; i += 4 ) {
    572 		_mm_store_ps( p + i, _mm_setzero_ps() );
    573 	}
    574 #else
    575 	memset( p, 0, length * sizeof( float ) );
    576 #endif
    577 }
    578 
    579 /*
    580 ========================
    581 idVecX::Random
    582 ========================
    583 */
    584 ID_INLINE void idVecX::Random( int seed, float l, float u ) {
    585 	idRandom rnd( seed );
    586 
    587 	float c = u - l;
    588 	for ( int i = 0; i < size; i++ ) {
    589 		p[i] = l + rnd.RandomFloat() * c;
    590 	}
    591 }
    592 
    593 /*
    594 ========================
    595 idVecX::Random
    596 ========================
    597 */
    598 ID_INLINE void idVecX::Random( int length, int seed, float l, float u ) {
    599 	idRandom rnd( seed );
    600 
    601 	SetSize( length );
    602 	float c = u - l;
    603 	for ( int i = 0; i < size; i++ ) {
    604 		p[i] = l + rnd.RandomFloat() * c;
    605 	}
    606 }
    607 
    608 /*
    609 ========================
    610 idVecX::Negate
    611 ========================
    612 */
    613 ID_INLINE void idVecX::Negate() {
    614 #if defined(ID_WIN_X86_SSE_INTRIN) && defined(VECX_SIMD)
    615 	ALIGN16( const unsigned int signBit[4] ) = { IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK };
    616 	for ( int i = 0; i < size; i += 4 ) {
    617 		_mm_store_ps( p + i, _mm_xor_ps( _mm_load_ps( p + i ), (__m128 &) signBit[0] ) );
    618 	}
    619 #else
    620 	for ( int i = 0; i < size; i++ ) {
    621 		p[i] = -p[i];
    622 	}
    623 #endif
    624 }
    625 
    626 /*
    627 ========================
    628 idVecX::Clamp
    629 ========================
    630 */
    631 ID_INLINE void idVecX::Clamp( float min, float max ) {
    632 	for ( int i = 0; i < size; i++ ) {
    633 		if ( p[i] < min ) {
    634 			p[i] = min;
    635 		} else if ( p[i] > max ) {
    636 			p[i] = max;
    637 		}
    638 	}
    639 }
    640 
    641 /*
    642 ========================
    643 idVecX::SwapElements
    644 ========================
    645 */
    646 ID_INLINE idVecX &idVecX::SwapElements( int e1, int e2 ) {
    647 	float tmp;
    648 	tmp = p[e1];
    649 	p[e1] = p[e2];
    650 	p[e2] = tmp;
    651 	return *this;
    652 }
    653 
    654 /*
    655 ========================
    656 idVecX::Length
    657 ========================
    658 */
    659 ID_INLINE float idVecX::Length() const {
    660 	float sum = 0.0f;
    661 	for ( int i = 0; i < size; i++ ) {
    662 		sum += p[i] * p[i];
    663 	}
    664 	return idMath::Sqrt( sum );
    665 }
    666 
    667 /*
    668 ========================
    669 idVecX::LengthSqr
    670 ========================
    671 */
    672 ID_INLINE float idVecX::LengthSqr() const {
    673 	float sum = 0.0f;
    674 	for ( int i = 0; i < size; i++ ) {
    675 		sum += p[i] * p[i];
    676 	}
    677 	return sum;
    678 }
    679 
    680 /*
    681 ========================
    682 idVecX::Normalize
    683 ========================
    684 */
    685 ID_INLINE idVecX idVecX::Normalize() const {
    686 	idVecX m;
    687 
    688 	m.SetTempSize( size );
    689 	float sum = 0.0f;
    690 	for ( int i = 0; i < size; i++ ) {
    691 		sum += p[i] * p[i];
    692 	}
    693 	float invSqrt = idMath::InvSqrt( sum );
    694 	for ( int i = 0; i < size; i++ ) {
    695 		m.p[i] = p[i] * invSqrt;
    696 	}
    697 	return m;
    698 }
    699 
    700 /*
    701 ========================
    702 idVecX::NormalizeSelf
    703 ========================
    704 */
    705 ID_INLINE float idVecX::NormalizeSelf() {
    706 	float sum = 0.0f;
    707 	for ( int i = 0; i < size; i++ ) {
    708 		sum += p[i] * p[i];
    709 	}
    710 	float invSqrt = idMath::InvSqrt( sum );
    711 	for ( int i = 0; i < size; i++ ) {
    712 		p[i] *= invSqrt;
    713 	}
    714 	return invSqrt * sum;
    715 }
    716 
    717 /*
    718 ========================
    719 idVecX::GetDimension
    720 ========================
    721 */
    722 ID_INLINE int idVecX::GetDimension() const {
    723 	return size;
    724 }
    725 
    726 /*
    727 ========================
    728 idVecX::SubVec3
    729 ========================
    730 */
    731 ID_INLINE idVec3 &idVecX::SubVec3( int index ) {
    732 	assert( index >= 0 && index * 3 + 3 <= size );
    733 	return *reinterpret_cast<idVec3 *>(p + index * 3);
    734 }
    735 
    736 /*
    737 ========================
    738 idVecX::SubVec3
    739 ========================
    740 */
    741 ID_INLINE const idVec3 &idVecX::SubVec3( int index ) const {
    742 	assert( index >= 0 && index * 3 + 3 <= size );
    743 	return *reinterpret_cast<const idVec3 *>(p + index * 3);
    744 }
    745 
    746 /*
    747 ========================
    748 idVecX::SubVec6
    749 ========================
    750 */
    751 ID_INLINE idVec6 &idVecX::SubVec6( int index ) {
    752 	assert( index >= 0 && index * 6 + 6 <= size );
    753 	return *reinterpret_cast<idVec6 *>(p + index * 6);
    754 }
    755 
    756 /*
    757 ========================
    758 idVecX::SubVec6
    759 ========================
    760 */
    761 ID_INLINE const idVec6 &idVecX::SubVec6( int index ) const {
    762 	assert( index >= 0 && index * 6 + 6 <= size );
    763 	return *reinterpret_cast<const idVec6 *>(p + index * 6);
    764 }
    765 
    766 /*
    767 ========================
    768 idVecX::ToFloatPtr
    769 ========================
    770 */
    771 ID_INLINE const float *idVecX::ToFloatPtr() const {
    772 	return p;
    773 }
    774 
    775 /*
    776 ========================
    777 idVecX::ToFloatPtr
    778 ========================
    779 */
    780 ID_INLINE float *idVecX::ToFloatPtr() {
    781 	return p;
    782 }
    783 
    784 /*
    785 ========================
    786 idVecX::AddScaleAdd
    787 ========================
    788 */
    789 ID_INLINE void idVecX::AddScaleAdd( const float scale, const idVecX &v0, const idVecX &v1 ) {
    790 	assert( GetSize() == v0.GetSize() );
    791 	assert( GetSize() == v1.GetSize() );
    792 
    793 	const float * v0Ptr = v0.ToFloatPtr();
    794 	const float * v1Ptr = v1.ToFloatPtr();
    795 	float * dstPtr = ToFloatPtr();
    796 
    797 	for ( int i = 0; i < size; i++ ) {
    798 		dstPtr[i] += scale * ( v0Ptr[i] + v1Ptr[i] );
    799 	}
    800 }
    801 
    802 #endif // !__MATH_VECTORX_H__