MatX.cpp - DOOM-3-BFG - DOOM 3 BFG Edition

MatX.cpp (114641B)
      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 #pragma hdrstop
     30 #include "../precompiled.h"
     31 
     32 //===============================================================
     33 //
     34 //  idMatX
     35 //
     36 //===============================================================
     37 
     38 float	idMatX::temp[MATX_MAX_TEMP+4];
     39 float *	idMatX::tempPtr = (float *) ( ( (int) idMatX::temp + 15 ) & ~15 );
     40 int		idMatX::tempIndex = 0;
     41 
     42 
     43 /*
     44 ============
     45 idMatX::ChangeSize
     46 ============
     47 */
     48 void idMatX::ChangeSize( int rows, int columns, bool makeZero ) {
     49 	int alloc = ( rows * columns + 3 ) & ~3;
     50 	if ( alloc > alloced && alloced != -1 ) {
     51 		float *oldMat = mat;
     52 		mat = (float *) Mem_Alloc16( alloc * sizeof( float ), TAG_MATH );
     53 		if ( makeZero ) {
     54 			memset( mat, 0, alloc * sizeof( float ) );
     55 		}
     56 		alloced = alloc;
     57 		if ( oldMat ) {
     58 			int minRow = Min( numRows, rows );
     59 			int minColumn = Min( numColumns, columns );
     60 			for ( int i = 0; i < minRow; i++ ) {
     61 				for ( int j = 0; j < minColumn; j++ ) {
     62 					mat[ i * columns + j ] = oldMat[ i * numColumns + j ];
     63 				}
     64 			}
     65 			Mem_Free16( oldMat );
     66 		}
     67 	} else {
     68 		if ( columns < numColumns ) {
     69 			int minRow = Min( numRows, rows );
     70 			for ( int i = 0; i < minRow; i++ ) {
     71 				for ( int j = 0; j < columns; j++ ) {
     72 					mat[ i * columns + j ] = mat[ i * numColumns + j ];
     73 				}
     74 			}
     75 		} else if ( columns > numColumns ) {
     76 			for ( int i = Min( numRows, rows ) - 1; i >= 0; i-- ) {
     77 				if ( makeZero ) {
     78 					for ( int j = columns - 1; j >= numColumns; j-- ) {
     79 						mat[ i * columns + j ] = 0.0f;
     80 					}
     81 				}
     82 				for ( int j = numColumns - 1; j >= 0; j-- ) {
     83 					mat[ i * columns + j ] = mat[ i * numColumns + j ];
     84 				}
     85 			}
     86 		}
     87 		if ( makeZero && rows > numRows ) {
     88 			memset( mat + numRows * columns, 0, ( rows - numRows ) * columns * sizeof( float ) );
     89 		}
     90 	}
     91 	numRows = rows;
     92 	numColumns = columns;
     93 	MATX_CLEAREND();
     94 }
     95 
     96 /*
     97 ============
     98 idMatX::RemoveRow
     99 ============
    100 */
    101 idMatX &idMatX::RemoveRow( int r ) {
    102 	int i;
    103 
    104 	assert( r < numRows );
    105 
    106 	numRows--;
    107 
    108 	for ( i = r; i < numRows; i++ ) {
    109 		memcpy( &mat[i * numColumns], &mat[( i + 1 ) * numColumns], numColumns * sizeof( float ) );
    110 	}
    111 
    112 	return *this;
    113 }
    114 
    115 /*
    116 ============
    117 idMatX::RemoveColumn
    118 ============
    119 */
    120 idMatX &idMatX::RemoveColumn( int r ) {
    121 	int i;
    122 
    123 	assert( r < numColumns );
    124 
    125 	numColumns--;
    126 
    127 	for ( i = 0; i < numRows - 1; i++ ) {
    128 		memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
    129 	}
    130 	memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
    131 
    132 	return *this;
    133 }
    134 
    135 /*
    136 ============
    137 idMatX::RemoveRowColumn
    138 ============
    139 */
    140 idMatX &idMatX::RemoveRowColumn( int r ) {
    141 	int i;
    142 
    143 	assert( r < numRows && r < numColumns );
    144 
    145 	numRows--;
    146 	numColumns--;
    147 
    148 	if ( r > 0 ) {
    149 		for ( i = 0; i < r - 1; i++ ) {
    150 			memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
    151 		}
    152 		memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
    153 	}
    154 
    155 	memcpy( &mat[r * numColumns], &mat[( r + 1 ) * ( numColumns + 1 )], r * sizeof( float ) );
    156 
    157 	for ( i = r; i < numRows - 1; i++ ) {
    158 		memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
    159 	}
    160 	memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
    161 
    162 	return *this;
    163 }
    164 
    165 /*
    166 ========================
    167 idMatX::CopyLowerToUpperTriangle
    168 ========================
    169 */
    170 void idMatX::CopyLowerToUpperTriangle() {
    171 	assert( ( GetNumColumns() & 3 ) == 0 );
    172 	assert( GetNumColumns() >= GetNumRows() );
    173 
    174 #ifdef ID_WIN_X86_SSE_INTRIN
    175 
    176 	const int n = GetNumColumns();
    177 	const int m = GetNumRows();
    178 
    179 	const int n0 = 0;
    180 	const int n1 = n;
    181 	const int n2 = ( n << 1 );
    182 	const int n3 = ( n << 1 ) + n;
    183 	const int n4 = ( n << 2 );
    184 
    185 	const int b1 = ( ( m - 0 ) >> 1 ) & 1;	// ( m & 3 ) > 1
    186 	const int b2 = ( ( m - 1 ) >> 1 ) & 1;	// ( m & 3 ) > 2 (provided ( m & 3 ) > 0)
    187 
    188 	const int n1_masked = ( n & -b1 );
    189 	const int n2_masked = ( n & -b1 ) + ( n & -b2 );
    190 
    191 	const __m128 mask0 = __m128c( _mm_set_epi32(  0,  0,  0, -1 ) );
    192 	const __m128 mask1 = __m128c( _mm_set_epi32(  0,  0, -1, -1 ) );
    193 	const __m128 mask2 = __m128c( _mm_set_epi32(  0, -1, -1, -1 ) );
    194 	const __m128 mask3 = __m128c( _mm_set_epi32( -1, -1, -1, -1 ) );
    195 
    196 	const __m128 bottomMask[2] = { __m128c( _mm_set1_epi32( 0 ) ), __m128c( _mm_set1_epi32( -1 ) ) };
    197 
    198 	float * __restrict basePtr = ToFloatPtr();
    199 
    200 	for ( int i = 0; i < m - 3; i += 4 ) {
    201 
    202 		// copy top left diagonal 4x4 block elements
    203 		__m128 r0 = _mm_and_ps( _mm_load_ps( basePtr + n0 ), mask0 );
    204 		__m128 r1 = _mm_and_ps( _mm_load_ps( basePtr + n1 ), mask1 );
    205 		__m128 r2 = _mm_and_ps( _mm_load_ps( basePtr + n2 ), mask2 );
    206 		__m128 r3 = _mm_and_ps( _mm_load_ps( basePtr + n3 ), mask3 );
    207 
    208 		__m128 t0 = _mm_unpacklo_ps( r0, r2 );	// x0, z0, x1, z1
    209 		__m128 t1 = _mm_unpackhi_ps( r0, r2 );	// x2, z2, x3, z3
    210 		__m128 t2 = _mm_unpacklo_ps( r1, r3 );	// y0, w0, y1, w1
    211 		__m128 t3 = _mm_unpackhi_ps( r1, r3 );	// y2, w2, y3, w3
    212 
    213 		__m128 s0 = _mm_unpacklo_ps( t0, t2 );	// x0, y0, z0, w0
    214 		__m128 s1 = _mm_unpackhi_ps( t0, t2 );	// x1, y1, z1, w1
    215 		__m128 s2 = _mm_unpacklo_ps( t1, t3 );	// x2, y2, z2, w2
    216 		__m128 s3 = _mm_unpackhi_ps( t1, t3 );	// x3, y3, z3, w3
    217 
    218 		r0 = _mm_or_ps( r0, s0 );
    219 		r1 = _mm_or_ps( r1, s1 );
    220 		r2 = _mm_or_ps( r2, s2 );
    221 		r3 = _mm_or_ps( r3, s3 );
    222 
    223 		_mm_store_ps( basePtr + n0, r0 );
    224 		_mm_store_ps( basePtr + n1, r1 );
    225 		_mm_store_ps( basePtr + n2, r2 );
    226 		_mm_store_ps( basePtr + n3, r3 );
    227 
    228 		// copy one column of 4x4 blocks to one row of 4x4 blocks
    229 		const float * __restrict srcPtr = basePtr;
    230 		float * __restrict dstPtr = basePtr;
    231 
    232 		for ( int j = i + 4; j < m - 3; j += 4 ) {
    233 			srcPtr += n4;
    234 			dstPtr += 4;
    235 
    236 			__m128 r0 = _mm_load_ps( srcPtr + n0 );
    237 			__m128 r1 = _mm_load_ps( srcPtr + n1 );
    238 			__m128 r2 = _mm_load_ps( srcPtr + n2 );
    239 			__m128 r3 = _mm_load_ps( srcPtr + n3 );
    240 
    241 			__m128 t0 = _mm_unpacklo_ps( r0, r2 );	// x0, z0, x1, z1
    242 			__m128 t1 = _mm_unpackhi_ps( r0, r2 );	// x2, z2, x3, z3
    243 			__m128 t2 = _mm_unpacklo_ps( r1, r3 );	// y0, w0, y1, w1
    244 			__m128 t3 = _mm_unpackhi_ps( r1, r3 );	// y2, w2, y3, w3
    245 
    246 			r0 = _mm_unpacklo_ps( t0, t2 );			// x0, y0, z0, w0
    247 			r1 = _mm_unpackhi_ps( t0, t2 );			// x1, y1, z1, w1
    248 			r2 = _mm_unpacklo_ps( t1, t3 );			// x2, y2, z2, w2
    249 			r3 = _mm_unpackhi_ps( t1, t3 );			// x3, y3, z3, w3
    250 
    251 			_mm_store_ps( dstPtr + n0, r0 );
    252 			_mm_store_ps( dstPtr + n1, r1 );
    253 			_mm_store_ps( dstPtr + n2, r2 );
    254 			_mm_store_ps( dstPtr + n3, r3 );
    255 		}
    256 
    257 		// copy the last partial 4x4 block elements
    258 		if ( m & 3 ) {
    259 			srcPtr += n4;
    260 			dstPtr += 4;
    261 
    262 			__m128 r0 = _mm_load_ps( srcPtr + n0 );
    263 			__m128 r1 = _mm_and_ps( _mm_load_ps( srcPtr + n1_masked ), bottomMask[b1] );
    264 			__m128 r2 = _mm_and_ps( _mm_load_ps( srcPtr + n2_masked ), bottomMask[b2] );
    265 			__m128 r3 = _mm_setzero_ps();
    266 
    267 			__m128 t0 = _mm_unpacklo_ps( r0, r2 );	// x0, z0, x1, z1
    268 			__m128 t1 = _mm_unpackhi_ps( r0, r2 );	// x2, z2, x3, z3
    269 			__m128 t2 = _mm_unpacklo_ps( r1, r3 );	// y0, w0, y1, w1
    270 			__m128 t3 = _mm_unpackhi_ps( r1, r3 );	// y2, w2, y3, w3
    271 
    272 			r0 = _mm_unpacklo_ps( t0, t2 );			// x0, y0, z0, w0
    273 			r1 = _mm_unpackhi_ps( t0, t2 );			// x1, y1, z1, w1
    274 			r2 = _mm_unpacklo_ps( t1, t3 );			// x2, y2, z2, w2
    275 			r3 = _mm_unpackhi_ps( t1, t3 );			// x3, y3, z3, w3
    276 
    277 			_mm_store_ps( dstPtr + n0, r0 );
    278 			_mm_store_ps( dstPtr + n1, r1 );
    279 			_mm_store_ps( dstPtr + n2, r2 );
    280 			_mm_store_ps( dstPtr + n3, r3 );
    281 		}
    282 
    283 		basePtr += n4 + 4;
    284 	}
    285 
    286 	// copy the lower right partial diagonal 4x4 block elements
    287 	if ( m & 3 ) {
    288 		__m128 r0 = _mm_and_ps( _mm_load_ps( basePtr + n0 ), mask0 );
    289 		__m128 r1 = _mm_and_ps( _mm_load_ps( basePtr + n1_masked ), _mm_and_ps( mask1, bottomMask[b1] ) );
    290 		__m128 r2 = _mm_and_ps( _mm_load_ps( basePtr + n2_masked ), _mm_and_ps( mask2, bottomMask[b2] ) );
    291 		__m128 r3 = _mm_setzero_ps();
    292 
    293 		__m128 t0 = _mm_unpacklo_ps( r0, r2 );	// x0, z0, x1, z1
    294 		__m128 t1 = _mm_unpackhi_ps( r0, r2 );	// x2, z2, x3, z3
    295 		__m128 t2 = _mm_unpacklo_ps( r1, r3 );	// y0, w0, y1, w1
    296 		__m128 t3 = _mm_unpackhi_ps( r1, r3 );	// y2, w2, y3, w3
    297 
    298 		__m128 s0 = _mm_unpacklo_ps( t0, t2 );	// x0, y0, z0, w0
    299 		__m128 s1 = _mm_unpackhi_ps( t0, t2 );	// x1, y1, z1, w1
    300 		__m128 s2 = _mm_unpacklo_ps( t1, t3 );	// x2, y2, z2, w2
    301 
    302 		r0 = _mm_or_ps( r0, s0 );
    303 		r1 = _mm_or_ps( r1, s1 );
    304 		r2 = _mm_or_ps( r2, s2 );
    305 
    306 		_mm_store_ps( basePtr + n2_masked, r2 );
    307 		_mm_store_ps( basePtr + n1_masked, r1 );
    308 		_mm_store_ps( basePtr + n0, r0 );
    309 	}
    310 
    311 #else
    312 
    313 	const int n = GetNumColumns();
    314 	const int m = GetNumRows();
    315 	for ( int i = 0; i < m; i++ ) {
    316 		const float * __restrict ptr = ToFloatPtr() + ( i + 1 ) * n + i;
    317 		float * __restrict dstPtr = ToFloatPtr() + i * n;
    318 		for ( int j = i + 1; j < m; j++ ) {
    319 			dstPtr[j] = ptr[0];
    320 			ptr += n;
    321 		}
    322 	}
    323 
    324 #endif
    325 
    326 #ifdef _DEBUG
    327 	for ( int i = 0; i < numRows; i++ ) {
    328 		for ( int j = 0; j < numRows; j++ ) {
    329 			assert( mat[ i * numColumns + j ] == mat[ j * numColumns + i ] );
    330 		}
    331 	}
    332 #endif
    333 }
    334 /*
    335 ============
    336 idMatX::IsOrthogonal
    337 
    338   returns true if (*this) * this->Transpose() == Identity
    339 ============
    340 */
    341 bool idMatX::IsOrthogonal( const float epsilon ) const {
    342 	float *ptr1, *ptr2, sum;
    343 
    344 	if ( !IsSquare() ) {
    345 		return false;
    346 	}
    347 
    348 	ptr1 = mat;
    349 	for ( int i = 0; i < numRows; i++ ) {
    350 		for ( int j = 0; j < numColumns; j++ ) {
    351 			ptr2 = mat + j;
    352 			sum = ptr1[0] * ptr2[0] - (float) ( i == j );
    353 			for ( int n = 1; n < numColumns; n++ ) {
    354 				ptr2 += numColumns;
    355 				sum += ptr1[n] * ptr2[0];
    356 			}
    357 			if ( idMath::Fabs( sum ) > epsilon ) {
    358 				return false;
    359 			}
    360 		}
    361 		ptr1 += numColumns;
    362 	}
    363 	return true;
    364 }
    365 
    366 /*
    367 ============
    368 idMatX::IsOrthonormal
    369 
    370   returns true if (*this) * this->Transpose() == Identity and the length of each column vector is 1
    371 ============
    372 */
    373 bool idMatX::IsOrthonormal( const float epsilon ) const {
    374 	float *ptr1, *ptr2, sum;
    375 
    376 	if ( !IsSquare() ) {
    377 		return false;
    378 	}
    379 
    380 	ptr1 = mat;
    381 	for ( int i = 0; i < numRows; i++ ) {
    382 		for ( int j = 0; j < numColumns; j++ ) {
    383 			ptr2 = mat + j;
    384 			sum = ptr1[0] * ptr2[0] - (float) ( i == j );
    385 			for ( int n = 1; n < numColumns; n++ ) {
    386 				ptr2 += numColumns;
    387 				sum += ptr1[n] * ptr2[0];
    388 			}
    389 			if ( idMath::Fabs( sum ) > epsilon ) {
    390 				return false;
    391 			}
    392 		}
    393 		ptr1 += numColumns;
    394 
    395 		ptr2 = mat + i;
    396 		sum = ptr2[0] * ptr2[0] - 1.0f;
    397 		for ( int j = 1; j < numRows; j++ ) {
    398 			ptr2 += numColumns;
    399 			sum += ptr2[j] * ptr2[j];
    400 		}
    401 		if ( idMath::Fabs( sum ) > epsilon ) {
    402 			return false;
    403 		}
    404 	}
    405 	return true;
    406 }
    407 
    408 /*
    409 ============
    410 idMatX::IsPMatrix
    411 
    412   returns true if the matrix is a P-matrix
    413   A square matrix is a P-matrix if all its principal minors are positive.
    414 ============
    415 */
    416 bool idMatX::IsPMatrix( const float epsilon ) const {
    417 	int i, j;
    418 	float d;
    419 	idMatX m;
    420 
    421 	if ( !IsSquare() ) {
    422 		return false;
    423 	}
    424 
    425 	if ( numRows <= 0 ) {
    426 		return true;
    427 	}
    428 
    429 	if ( (*this)[0][0] <= epsilon ) {
    430 		return false;
    431 	}
    432 
    433 	if ( numRows <= 1 ) {
    434 		return true;
    435 	}
    436 
    437 	m.SetData( numRows - 1, numColumns - 1, MATX_ALLOCA( ( numRows - 1 ) * ( numColumns - 1 ) ) );
    438 
    439 	for ( i = 1; i < numRows; i++ ) {
    440 		for ( j = 1; j < numColumns; j++ ) {
    441 			m[i-1][j-1] = (*this)[i][j];
    442 		}
    443 	}
    444 
    445 	if ( !m.IsPMatrix( epsilon ) ) {
    446 		return false;
    447 	}
    448 
    449 	for ( i = 1; i < numRows; i++ ) {
    450 		d = (*this)[i][0] / (*this)[0][0];
    451 		for ( j = 1; j < numColumns; j++ ) {
    452 			m[i-1][j-1] = (*this)[i][j] - d * (*this)[0][j];
    453 		}
    454 	}
    455 
    456 	if ( !m.IsPMatrix( epsilon ) ) {
    457 		return false;
    458 	}
    459 
    460 	return true;
    461 }
    462 
    463 /*
    464 ============
    465 idMatX::IsZMatrix
    466 
    467   returns true if the matrix is a Z-matrix
    468   A square matrix M is a Z-matrix if M[i][j] <= 0 for all i != j.
    469 ============
    470 */
    471 bool idMatX::IsZMatrix( const float epsilon ) const {
    472 	int i, j;
    473 
    474 	if ( !IsSquare() ) {
    475 		return false;
    476 	}
    477 
    478 	for ( i = 0; i < numRows; i++ ) {
    479 		for ( j = 0; j < numColumns; j++ ) {
    480 			if ( (*this)[i][j] > epsilon && i != j ) {
    481 				return false;
    482 			}
    483 		}
    484 	}
    485 	return true;
    486 }
    487 
    488 /*
    489 ============
    490 idMatX::IsPositiveDefinite
    491 
    492   returns true if the matrix is Positive Definite (PD)
    493   A square matrix M of order n is said to be PD if y'My > 0 for all vectors y of dimension n, y != 0.
    494 ============
    495 */
    496 bool idMatX::IsPositiveDefinite( const float epsilon ) const {
    497 	int i, j, k;
    498 	float d, s;
    499 	idMatX m;
    500 
    501 	// the matrix must be square
    502 	if ( !IsSquare() ) {
    503 		return false;
    504 	}
    505 
    506 	// copy matrix
    507 	m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
    508 	m = *this;
    509 
    510 	// add transpose
    511 	for ( i = 0; i < numRows; i++ ) {
    512 		for ( j = 0; j < numColumns; j++ ) {
    513 			m[i][j] += (*this)[j][i];
    514 		}
    515 	}
    516 
    517 	// test Positive Definiteness with Gaussian pivot steps
    518 	for ( i = 0; i < numRows; i++ ) {
    519 
    520 		for ( j = i; j < numColumns; j++ ) {
    521 			if ( m[j][j] <= epsilon ) {
    522 				return false;
    523 			}
    524 		}
    525 
    526 		d = 1.0f / m[i][i];
    527 		for ( j = i + 1; j < numColumns; j++ ) {
    528 			s = d * m[j][i];
    529 			m[j][i] = 0.0f;
    530 			for ( k = i + 1; k < numRows; k++ ) {
    531 				m[j][k] -= s * m[i][k];
    532 			}
    533 		}
    534 	}
    535 
    536 	return true;
    537 }
    538 
    539 /*
    540 ============
    541 idMatX::IsSymmetricPositiveDefinite
    542 
    543   returns true if the matrix is Symmetric Positive Definite (PD)
    544 ============
    545 */
    546 bool idMatX::IsSymmetricPositiveDefinite( const float epsilon ) const {
    547 	idMatX m;
    548 
    549 	// the matrix must be symmetric
    550 	if ( !IsSymmetric( epsilon ) ) {
    551 		return false;
    552 	}
    553 
    554 	// copy matrix
    555 	m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
    556 	m = *this;
    557 
    558 	// being able to obtain Cholesky factors is both a necessary and sufficient condition for positive definiteness
    559 	return m.Cholesky_Factor();
    560 }
    561 
    562 /*
    563 ============
    564 idMatX::IsPositiveSemiDefinite
    565 
    566   returns true if the matrix is Positive Semi Definite (PSD)
    567   A square matrix M of order n is said to be PSD if y'My >= 0 for all vectors y of dimension n, y != 0.
    568 ============
    569 */
    570 bool idMatX::IsPositiveSemiDefinite( const float epsilon ) const {
    571 	int i, j, k;
    572 	float d, s;
    573 	idMatX m;
    574 
    575 	// the matrix must be square
    576 	if ( !IsSquare() ) {
    577 		return false;
    578 	}
    579 
    580 	// copy original matrix
    581 	m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
    582 	m = *this;
    583 
    584 	// add transpose
    585 	for ( i = 0; i < numRows; i++ ) {
    586 		for ( j = 0; j < numColumns; j++ ) {
    587 			m[i][j] += (*this)[j][i];
    588 		}
    589 	}
    590 
    591 	// test Positive Semi Definiteness with Gaussian pivot steps
    592 	for ( i = 0; i < numRows; i++ ) {
    593 
    594 		for ( j = i; j < numColumns; j++ ) {
    595 			if ( m[j][j] < -epsilon ) {
    596 				return false;
    597 			}
    598 			if ( m[j][j] > epsilon ) {
    599 				continue;
    600 			}
    601 			for ( k = 0; k < numRows; k++ ) {
    602 				if ( idMath::Fabs( m[k][j] ) > epsilon ) {
    603 					return false;
    604 				}
    605 				if ( idMath::Fabs( m[j][k] ) > epsilon ) {
    606 					return false;
    607 				}
    608 			}
    609 		}
    610 
    611 		if ( m[i][i] <= epsilon ) {
    612 			continue;
    613 		}
    614 
    615 		d = 1.0f / m[i][i];
    616 		for ( j = i + 1; j < numColumns; j++ ) {
    617 			s = d * m[j][i];
    618 			m[j][i] = 0.0f;
    619 			for ( k = i + 1; k < numRows; k++ ) {
    620 				m[j][k] -= s * m[i][k];
    621 			}
    622 		}
    623 	}
    624 
    625 	return true;
    626 }
    627 
    628 /*
    629 ============
    630 idMatX::IsSymmetricPositiveSemiDefinite
    631 
    632   returns true if the matrix is Symmetric Positive Semi Definite (PSD)
    633 ============
    634 */
    635 bool idMatX::IsSymmetricPositiveSemiDefinite( const float epsilon ) const {
    636 
    637 	// the matrix must be symmetric
    638 	if ( !IsSymmetric( epsilon ) ) {
    639 		return false;
    640 	}
    641 
    642 	return IsPositiveSemiDefinite( epsilon );
    643 }
    644 
    645 /*
    646 ============
    647 idMatX::LowerTriangularInverse
    648 
    649   in-place inversion of the lower triangular matrix
    650 ============
    651 */
    652 bool idMatX::LowerTriangularInverse() {
    653 	int i, j, k;
    654 	double d, sum;
    655 
    656 	for ( i = 0; i < numRows; i++ ) {
    657 		d = (*this)[i][i];
    658 		if ( d == 0.0f ) {
    659 			return false;
    660 		}
    661 		(*this)[i][i] = d = 1.0f / d;
    662 
    663 		for ( j = 0; j < i; j++ ) {
    664 			sum = 0.0f;
    665 			for ( k = j; k < i; k++ ) {
    666 				sum -= (*this)[i][k] * (*this)[k][j];
    667 			}
    668 			(*this)[i][j] = sum * d;
    669 		}
    670 	}
    671 	return true;
    672 }
    673 
    674 /*
    675 ============
    676 idMatX::UpperTriangularInverse
    677 
    678   in-place inversion of the upper triangular matrix
    679 ============
    680 */
    681 bool idMatX::UpperTriangularInverse() {
    682 	int i, j, k;
    683 	double d, sum;
    684 
    685 	for ( i = numRows-1; i >= 0; i-- ) {
    686 		d = (*this)[i][i];
    687 		if ( d == 0.0f ) {
    688 			return false;
    689 		}
    690 		(*this)[i][i] = d = 1.0f / d;
    691 
    692 		for ( j = numRows-1; j > i; j-- ) {
    693 			sum = 0.0f;
    694 			for ( k = j; k > i; k-- ) {
    695 				sum -= (*this)[i][k] * (*this)[k][j];
    696 			}
    697 			(*this)[i][j] = sum * d;
    698 		}
    699 	}
    700 	return true;
    701 }
    702 
    703 /*
    704 =============
    705 idMatX::ToString
    706 =============
    707 */
    708 const char *idMatX::ToString( int precision ) const {
    709 	return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
    710 }
    711 
    712 /*
    713 ============
    714 idMatX::Update_RankOne
    715 
    716   Updates the matrix to obtain the matrix: A + alpha * v * w'
    717 ============
    718 */
    719 void idMatX::Update_RankOne( const idVecX &v, const idVecX &w, float alpha ) {
    720 	int i, j;
    721 	float s;
    722 
    723 	assert( v.GetSize() >= numRows );
    724 	assert( w.GetSize() >= numColumns );
    725 
    726 	for ( i = 0; i < numRows; i++ ) {
    727 		s = alpha * v[i];
    728 		for ( j = 0; j < numColumns; j++ ) {
    729 			(*this)[i][j] += s * w[j];
    730 		}
    731 	}
    732 }
    733 
    734 /*
    735 ============
    736 idMatX::Update_RankOneSymmetric
    737 
    738   Updates the matrix to obtain the matrix: A + alpha * v * v'
    739 ============
    740 */
    741 void idMatX::Update_RankOneSymmetric( const idVecX &v, float alpha ) {
    742 	int i, j;
    743 	float s;
    744 
    745 	assert( numRows == numColumns );
    746 	assert( v.GetSize() >= numRows );
    747 
    748 	for ( i = 0; i < numRows; i++ ) {
    749 		s = alpha * v[i];
    750 		for ( j = 0; j < numColumns; j++ ) {
    751 			(*this)[i][j] += s * v[j];
    752 		}
    753 	}
    754 }
    755 
    756 /*
    757 ============
    758 idMatX::Update_RowColumn
    759 
    760   Updates the matrix to obtain the matrix:
    761 
    762       [ 0  a  0 ]
    763   A + [ d  b  e ]
    764       [ 0  c  0 ]
    765 
    766   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
    767 ============
    768 */
    769 void idMatX::Update_RowColumn( const idVecX &v, const idVecX &w, int r ) {
    770 	int i;
    771 
    772 	assert( w[r] == 0.0f );
    773 	assert( v.GetSize() >= numColumns );
    774 	assert( w.GetSize() >= numRows );
    775 
    776 	for ( i = 0; i < numRows; i++ ) {
    777 		(*this)[i][r] += v[i];
    778 	}
    779 	for ( i = 0; i < numColumns; i++ ) {
    780 		(*this)[r][i] += w[i];
    781 	}
    782 }
    783 
    784 /*
    785 ============
    786 idMatX::Update_RowColumnSymmetric
    787 
    788   Updates the matrix to obtain the matrix:
    789 
    790       [ 0  a  0 ]
    791   A + [ a  b  c ]
    792       [ 0  c  0 ]
    793 
    794   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
    795 ============
    796 */
    797 void idMatX::Update_RowColumnSymmetric( const idVecX &v, int r ) {
    798 	int i;
    799 
    800 	assert( numRows == numColumns );
    801 	assert( v.GetSize() >= numRows );
    802 
    803 	for ( i = 0; i < r; i++ ) {
    804 		(*this)[i][r] += v[i];
    805 		(*this)[r][i] += v[i];
    806 	}
    807 	(*this)[r][r] += v[r];
    808 	for ( i = r+1; i < numRows; i++ ) {
    809 		(*this)[i][r] += v[i];
    810 		(*this)[r][i] += v[i];
    811 	}
    812 }
    813 
    814 /*
    815 ============
    816 idMatX::Update_Increment
    817 
    818   Updates the matrix to obtain the matrix:
    819 
    820   [ A  a ]
    821   [ c  b ]
    822 
    823   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1]], w[numColumns] = 0
    824 ============
    825 */
    826 void idMatX::Update_Increment( const idVecX &v, const idVecX &w ) {
    827 	int i;
    828 
    829 	assert( numRows == numColumns );
    830 	assert( v.GetSize() >= numRows+1 );
    831 	assert( w.GetSize() >= numColumns+1 );
    832 
    833 	ChangeSize( numRows+1, numColumns+1, false );
    834 
    835 	for ( i = 0; i < numRows; i++ ) {
    836 		(*this)[i][numColumns-1] = v[i];
    837 	}
    838 	for ( i = 0; i < numColumns-1; i++ ) {
    839 		(*this)[numRows-1][i] = w[i];
    840 	}
    841 }
    842 
    843 /*
    844 ============
    845 idMatX::Update_IncrementSymmetric
    846 
    847   Updates the matrix to obtain the matrix:
    848 
    849   [ A  a ]
    850   [ a  b ]
    851 
    852   where: a = v[0,numRows-1], b = v[numRows]
    853 ============
    854 */
    855 void idMatX::Update_IncrementSymmetric( const idVecX &v ) {
    856 	int i;
    857 
    858 	assert( numRows == numColumns );
    859 	assert( v.GetSize() >= numRows+1 );
    860 
    861 	ChangeSize( numRows+1, numColumns+1, false );
    862 
    863 	for ( i = 0; i < numRows-1; i++ ) {
    864 		(*this)[i][numColumns-1] = v[i];
    865 	}
    866 	for ( i = 0; i < numColumns; i++ ) {
    867 		(*this)[numRows-1][i] = v[i];
    868 	}
    869 }
    870 
    871 /*
    872 ============
    873 idMatX::Update_Decrement
    874 
    875   Updates the matrix to obtain a matrix with row r and column r removed.
    876 ============
    877 */
    878 void idMatX::Update_Decrement( int r ) {
    879 	RemoveRowColumn( r );
    880 }
    881 
    882 /*
    883 ============
    884 idMatX::Inverse_GaussJordan
    885 
    886   in-place inversion using Gauss-Jordan elimination
    887 ============
    888 */
    889 bool idMatX::Inverse_GaussJordan() {
    890 	int i, j, k, r, c;
    891 	float d, max;
    892 
    893 	assert( numRows == numColumns );
    894 
    895 	int *columnIndex = (int *) _alloca16( numRows * sizeof( int ) );
    896 	int *rowIndex = (int *) _alloca16( numRows * sizeof( int ) );
    897 	bool *pivot = (bool *) _alloca16( numRows * sizeof( bool ) );
    898 
    899 	memset( pivot, 0, numRows * sizeof( bool ) );
    900 
    901 	// elimination with full pivoting
    902 	for ( i = 0; i < numRows; i++ ) {
    903 
    904 		// search the whole matrix except for pivoted rows for the maximum absolute value
    905 		max = 0.0f;
    906 		r = c = 0;
    907 		for ( j = 0; j < numRows; j++ ) {
    908 			if ( !pivot[j] ) {
    909 				for ( k = 0; k < numRows; k++ ) {
    910 					if ( !pivot[k] ) {
    911 						d = idMath::Fabs( (*this)[j][k] );
    912 						if ( d > max ) {
    913 							max = d;
    914 							r = j;
    915 							c = k;
    916 						}
    917 					}
    918 				}
    919 			}
    920 		}
    921 
    922 		if ( max == 0.0f ) {
    923 			// matrix is not invertible
    924 			return false;
    925 		}
    926 
    927 		pivot[c] = true;
    928 
    929 		// swap rows such that entry (c,c) has the pivot entry
    930 		if ( r != c ) {
    931 			SwapRows( r, c );
    932 		}
    933 
    934 		// keep track of the row permutation
    935 		rowIndex[i] = r;
    936 		columnIndex[i] = c;
    937 
    938 		// scale the row to make the pivot entry equal to 1
    939 		d = 1.0f / (*this)[c][c];
    940 		(*this)[c][c] = 1.0f;
    941 		for ( k = 0; k < numRows; k++ ) {
    942 			(*this)[c][k] *= d;
    943 		}
    944 
    945 		// zero out the pivot column entries in the other rows
    946 		for ( j = 0; j < numRows; j++ ) {
    947 			if ( j != c ) {
    948 				d = (*this)[j][c];
    949 				(*this)[j][c] = 0.0f;
    950 				for ( k = 0; k < numRows; k++ ) {
    951 					(*this)[j][k] -= (*this)[c][k] * d;
    952 				}
    953 			}
    954 		}
    955 	}
    956 
    957 	// reorder rows to store the inverse of the original matrix
    958 	for ( j = numRows - 1; j >= 0; j-- ) {
    959 		if ( rowIndex[j] != columnIndex[j] ) {
    960 			for ( k = 0; k < numRows; k++ ) {
    961 				d = (*this)[k][rowIndex[j]];
    962 				(*this)[k][rowIndex[j]] = (*this)[k][columnIndex[j]];
    963 				(*this)[k][columnIndex[j]] = d;
    964 			}
    965 		}
    966 	}
    967 
    968 	return true;
    969 }
    970 
    971 /*
    972 ============
    973 idMatX::Inverse_UpdateRankOne
    974 
    975   Updates the in-place inverse using the Sherman-Morrison formula to obtain the inverse for the matrix: A + alpha * v * w'
    976 ============
    977 */
    978 bool idMatX::Inverse_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha ) {
    979 	int i, j;
    980 	float beta, s;
    981 	idVecX y, z;
    982 
    983 	assert( numRows == numColumns );
    984 	assert( v.GetSize() >= numColumns );
    985 	assert( w.GetSize() >= numRows );
    986 
    987 	y.SetData( numRows, VECX_ALLOCA( numRows ) );
    988 	z.SetData( numRows, VECX_ALLOCA( numRows ) );
    989 
    990 	Multiply( y, v );
    991 	TransposeMultiply( z, w );
    992 	beta = 1.0f + ( w * y );
    993 
    994 	if ( beta == 0.0f ) {
    995 		return false;
    996 	}
    997 
    998 	alpha /= beta;
    999 
   1000 	for ( i = 0; i < numRows; i++ ) {
   1001 		s = y[i] * alpha;
   1002 		for ( j = 0; j < numColumns; j++ ) {
   1003 			(*this)[i][j] -= s * z[j];
   1004 		}
   1005 	}
   1006 	return true;
   1007 }
   1008 
   1009 /*
   1010 ============
   1011 idMatX::Inverse_UpdateRowColumn
   1012 
   1013   Updates the in-place inverse to obtain the inverse for the matrix:
   1014 
   1015       [ 0  a  0 ]
   1016   A + [ d  b  e ]
   1017       [ 0  c  0 ]
   1018 
   1019   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
   1020 ============
   1021 */
   1022 bool idMatX::Inverse_UpdateRowColumn( const idVecX &v, const idVecX &w, int r ) {
   1023 	idVecX s;
   1024 
   1025 	assert( numRows == numColumns );
   1026 	assert( v.GetSize() >= numColumns );
   1027 	assert( w.GetSize() >= numRows );
   1028 	assert( r >= 0 && r < numRows && r < numColumns );
   1029 	assert( w[r] == 0.0f );
   1030 
   1031 	s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
   1032 	s.Zero();
   1033 	s[r] = 1.0f;
   1034 
   1035 	if ( !Inverse_UpdateRankOne( v, s, 1.0f ) ) {
   1036 		return false;
   1037 	}
   1038 	if ( !Inverse_UpdateRankOne( s, w, 1.0f ) ) {
   1039 		return false;
   1040 	}
   1041 	return true;
   1042 }
   1043 
   1044 /*
   1045 ============
   1046 idMatX::Inverse_UpdateIncrement
   1047 
   1048   Updates the in-place inverse to obtain the inverse for the matrix:
   1049 
   1050   [ A  a ]
   1051   [ c  b ]
   1052 
   1053   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
   1054 ============
   1055 */
   1056 bool idMatX::Inverse_UpdateIncrement( const idVecX &v, const idVecX &w ) {
   1057 	idVecX v2;
   1058 
   1059 	assert( numRows == numColumns );
   1060 	assert( v.GetSize() >= numRows+1 );
   1061 	assert( w.GetSize() >= numColumns+1 );
   1062 
   1063 	ChangeSize( numRows+1, numColumns+1, true );
   1064 	(*this)[numRows-1][numRows-1] = 1.0f;
   1065 
   1066 	v2.SetData( numRows, VECX_ALLOCA( numRows ) );
   1067 	v2 = v;
   1068 	v2[numRows-1] -= 1.0f;
   1069 
   1070 	return Inverse_UpdateRowColumn( v2, w, numRows-1 );
   1071 }
   1072 
   1073 /*
   1074 ============
   1075 idMatX::Inverse_UpdateDecrement
   1076 
   1077   Updates the in-place inverse to obtain the inverse of the matrix with row r and column r removed.
   1078   v and w should store the column and row of the original matrix respectively.
   1079 ============
   1080 */
   1081 bool idMatX::Inverse_UpdateDecrement( const idVecX &v, const idVecX &w, int r ) {
   1082 	idVecX v1, w1;
   1083 
   1084 	assert( numRows == numColumns );
   1085 	assert( v.GetSize() >= numRows );
   1086 	assert( w.GetSize() >= numColumns );
   1087 	assert( r >= 0 && r < numRows && r < numColumns );
   1088 
   1089 	v1.SetData( numRows, VECX_ALLOCA( numRows ) );
   1090 	w1.SetData( numRows, VECX_ALLOCA( numRows ) );
   1091 
   1092 	// update the row and column to identity
   1093 	v1 = -v;
   1094 	w1 = -w;
   1095 	v1[r] += 1.0f;
   1096 	w1[r] = 0.0f;
   1097 
   1098 	if ( !Inverse_UpdateRowColumn( v1, w1, r ) ) {
   1099 		return false;
   1100 	}
   1101 
   1102 	// physically remove the row and column
   1103 	Update_Decrement( r );
   1104 
   1105 	return true;
   1106 }
   1107 
   1108 /*
   1109 ============
   1110 idMatX::Inverse_Solve
   1111 
   1112   Solve Ax = b with A inverted
   1113 ============
   1114 */
   1115 void idMatX::Inverse_Solve( idVecX &x, const idVecX &b ) const {
   1116 	Multiply( x, b );
   1117 }
   1118 
   1119 /*
   1120 ============
   1121 idMatX::LU_Factor
   1122 
   1123   in-place factorization: LU
   1124   L is a triangular matrix stored in the lower triangle.
   1125   L has ones on the diagonal that are not stored.
   1126   U is a triangular matrix stored in the upper triangle.
   1127   If index != NULL partial pivoting is used for numerical stability.
   1128   If index != NULL it must point to an array of numRow integers and is used to keep track of the row permutation.
   1129   If det != NULL the determinant of the matrix is calculated and stored.
   1130 ============
   1131 */
   1132 bool idMatX::LU_Factor( int *index, float *det ) {
   1133 	int i, j, k, newi, min;
   1134 	double s, t, d, w;
   1135 
   1136 	// if partial pivoting should be used
   1137 	if ( index ) {
   1138 		for ( i = 0; i < numRows; i++ ) {
   1139 			index[i] = i;
   1140 		}
   1141 	}
   1142 
   1143 	w = 1.0f;
   1144 	min = Min( numRows, numColumns );
   1145 	for ( i = 0; i < min; i++ ) {
   1146 
   1147 		newi = i;
   1148 		s = idMath::Fabs( (*this)[i][i] );
   1149 
   1150 		if ( index ) {
   1151 			// find the largest absolute pivot
   1152 			for ( j = i + 1; j < numRows; j++ ) {
   1153 				t = idMath::Fabs( (*this)[j][i] );
   1154 				if ( t > s ) {
   1155 					newi = j;
   1156 					s = t;
   1157 				}
   1158 			}
   1159 		}
   1160 
   1161 		if ( s == 0.0f ) {
   1162 			return false;
   1163 		}
   1164 
   1165 		if ( newi != i && index ) {
   1166 
   1167 			w = -w;
   1168 
   1169 			// swap index elements
   1170 			k = index[i];
   1171 			index[i] = index[newi];
   1172 			index[newi] = k;
   1173 
   1174 			// swap rows
   1175 			for ( j = 0; j < numColumns; j++ ) {
   1176 				t = (*this)[newi][j];
   1177 				(*this)[newi][j] = (*this)[i][j];
   1178 				(*this)[i][j] = t;
   1179 			}
   1180 		}
   1181 
   1182 		if ( i < numRows ) {
   1183 			d = 1.0f / (*this)[i][i];
   1184 			for ( j = i + 1; j < numRows; j++ ) {
   1185 				(*this)[j][i] *= d;
   1186 			}
   1187 		}
   1188 
   1189 		if ( i < min-1 ) {
   1190 			for ( j = i + 1; j < numRows; j++ ) {
   1191 				d = (*this)[j][i];
   1192 				for ( k = i + 1; k < numColumns; k++ ) {
   1193 					(*this)[j][k] -= d * (*this)[i][k];
   1194 				}
   1195 			}
   1196 		}
   1197 	}
   1198 
   1199 	if ( det ) {
   1200 		for ( i = 0; i < numRows; i++ ) {
   1201 			w *= (*this)[i][i];
   1202 		}
   1203 		*det = w;
   1204 	}
   1205 
   1206 	return true;
   1207 }   
   1208 
   1209 /*
   1210 ============
   1211 idMatX::LU_UpdateRankOne
   1212 
   1213   Updates the in-place LU factorization to obtain the factors for the matrix: LU + alpha * v * w'
   1214 ============
   1215 */
   1216 bool idMatX::LU_UpdateRankOne( const idVecX &v, const idVecX &w, float alpha, int *index ) {
   1217 	int i, j, max;
   1218 	float *y, *z;
   1219 	double diag, beta, p0, p1, d;
   1220 
   1221 	assert( v.GetSize() >= numColumns );
   1222 	assert( w.GetSize() >= numRows );
   1223 
   1224 	y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
   1225 	z = (float *) _alloca16( w.GetSize() * sizeof( float ) );
   1226 
   1227 	if ( index != NULL ) {
   1228 		for ( i = 0; i < numRows; i++ ) {
   1229 			y[i] = alpha * v[index[i]];
   1230 		}
   1231 	} else {
   1232 		for ( i = 0; i < numRows; i++ ) {
   1233 			y[i] = alpha * v[i];
   1234 		}
   1235 	}
   1236 
   1237 	memcpy( z, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
   1238 
   1239 	max = Min( numRows, numColumns );
   1240 	for ( i = 0; i < max; i++ ) {
   1241 		diag = (*this)[i][i];
   1242 
   1243 		p0 = y[i];
   1244 		p1 = z[i];
   1245 		diag += p0 * p1;
   1246 
   1247 		if ( diag == 0.0f ) {
   1248 			return false;
   1249 		}
   1250 
   1251 		beta = p1 / diag;
   1252 
   1253 		(*this)[i][i] = diag;
   1254 
   1255 		for ( j = i+1; j < numColumns; j++ ) {
   1256 
   1257 			d = (*this)[i][j];
   1258 
   1259 			d += p0 * z[j];
   1260 			z[j] -= beta * d;
   1261 
   1262 			(*this)[i][j] = d;
   1263 		}
   1264 
   1265 		for ( j = i+1; j < numRows; j++ ) {
   1266 
   1267 			d = (*this)[j][i];
   1268 
   1269 			y[j] -= p0 * d;
   1270 			d += beta * y[j];
   1271 
   1272 			(*this)[j][i] = d;
   1273 		}
   1274 	}
   1275 	return true;
   1276 }
   1277 
   1278 /*
   1279 ============
   1280 idMatX::LU_UpdateRowColumn
   1281 
   1282   Updates the in-place LU factorization to obtain the factors for the matrix:
   1283 
   1284        [ 0  a  0 ]
   1285   LU + [ d  b  e ]
   1286        [ 0  c  0 ]
   1287 
   1288   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
   1289 ============
   1290 */
   1291 bool idMatX::LU_UpdateRowColumn( const idVecX &v, const idVecX &w, int r, int *index ) {
   1292 #if 0
   1293 
   1294 	idVecX s;
   1295 
   1296 	assert( v.GetSize() >= numColumns );
   1297 	assert( w.GetSize() >= numRows );
   1298 	assert( r >= 0 && r < numRows && r < numColumns );
   1299 	assert( w[r] == 0.0f );
   1300 
   1301 	s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
   1302 	s.Zero();
   1303 	s[r] = 1.0f;
   1304 
   1305 	if ( !LU_UpdateRankOne( v, s, 1.0f, index ) ) {
   1306 		return false;
   1307 	}
   1308 	if ( !LU_UpdateRankOne( s, w, 1.0f, index ) ) {
   1309 		return false;
   1310 	}
   1311 	return true;
   1312 
   1313 #else
   1314 
   1315 	int i, j, min, max, rp;
   1316 	float *y0, *y1, *z0, *z1;
   1317 	double diag, beta0, beta1, p0, p1, q0, q1, d;
   1318 
   1319 	assert( v.GetSize() >= numColumns );
   1320 	assert( w.GetSize() >= numRows );
   1321 	assert( r >= 0 && r < numColumns && r < numRows );
   1322 	assert( w[r] == 0.0f );
   1323 
   1324 	y0 = (float *) _alloca16( v.GetSize() * sizeof( float ) );
   1325 	z0 = (float *) _alloca16( w.GetSize() * sizeof( float ) );
   1326 	y1 = (float *) _alloca16( v.GetSize() * sizeof( float ) );
   1327 	z1 = (float *) _alloca16( w.GetSize() * sizeof( float ) );
   1328 
   1329 	if ( index != NULL ) {
   1330 		for ( i = 0; i < numRows; i++ ) {
   1331 			y0[i] = v[index[i]];
   1332 		}
   1333 		rp = r;
   1334 		for ( i = 0; i < numRows; i++ ) {
   1335 			if ( index[i] == r ) {
   1336 				rp = i;
   1337 				break;
   1338 			}
   1339 		}
   1340 	} else {
   1341 		memcpy( y0, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
   1342 		rp = r;
   1343 	}
   1344 
   1345 	memset( y1, 0, v.GetSize() * sizeof( float ) );
   1346 	y1[rp] = 1.0f;
   1347 
   1348 	memset( z0, 0, w.GetSize() * sizeof( float ) );
   1349 	z0[r] = 1.0f;
   1350 
   1351 	memcpy( z1, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
   1352 
   1353 	// update the beginning of the to be updated row and column
   1354 	min = Min( r, rp );
   1355 	for ( i = 0; i < min; i++ ) {
   1356 		p0 = y0[i];
   1357 		beta1 = z1[i] / (*this)[i][i];
   1358 
   1359 		(*this)[i][r] += p0;
   1360 		for ( j = i+1; j < numColumns; j++ ) {
   1361 			z1[j] -= beta1 * (*this)[i][j];
   1362 		}
   1363 		for ( j = i+1; j < numRows; j++ ) {
   1364 			y0[j] -= p0 * (*this)[j][i];
   1365 		}
   1366 		(*this)[rp][i] += beta1;
   1367 	}
   1368 
   1369 	// update the lower right corner starting at r,r
   1370 	max = Min( numRows, numColumns );
   1371 	for ( i = min; i < max; i++ ) {
   1372 		diag = (*this)[i][i];
   1373 
   1374 		p0 = y0[i];
   1375 		p1 = z0[i];
   1376 		diag += p0 * p1;
   1377 
   1378 		if ( diag == 0.0f ) {
   1379 			return false;
   1380 		}
   1381 
   1382 		beta0 = p1 / diag;
   1383 
   1384 		q0 = y1[i];
   1385 		q1 = z1[i];
   1386 		diag += q0 * q1;
   1387 
   1388 		if ( diag == 0.0f ) {
   1389 			return false;
   1390 		}
   1391 
   1392 		beta1 = q1 / diag;
   1393 
   1394 		(*this)[i][i] = diag;
   1395 
   1396 		for ( j = i+1; j < numColumns; j++ ) {
   1397 
   1398 			d = (*this)[i][j];
   1399 
   1400 			d += p0 * z0[j];
   1401 			z0[j] -= beta0 * d;
   1402 
   1403 			d += q0 * z1[j];
   1404 			z1[j] -= beta1 * d;
   1405 
   1406 			(*this)[i][j] = d;
   1407 		}
   1408 
   1409 		for ( j = i+1; j < numRows; j++ ) {
   1410 
   1411 			d = (*this)[j][i];
   1412 
   1413 			y0[j] -= p0 * d;
   1414 			d += beta0 * y0[j];
   1415 
   1416 			y1[j] -= q0 * d;
   1417 			d += beta1 * y1[j];
   1418 
   1419 			(*this)[j][i] = d;
   1420 		}
   1421 	}
   1422 	return true;
   1423 
   1424 #endif
   1425 }
   1426 
   1427 /*
   1428 ============
   1429 idMatX::LU_UpdateIncrement
   1430 
   1431   Updates the in-place LU factorization to obtain the factors for the matrix:
   1432 
   1433   [ A  a ]
   1434   [ c  b ]
   1435 
   1436   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
   1437 ============
   1438 */
   1439 bool idMatX::LU_UpdateIncrement( const idVecX &v, const idVecX &w, int *index ) {
   1440 	int i, j;
   1441 	float sum;
   1442 
   1443 	assert( numRows == numColumns );
   1444 	assert( v.GetSize() >= numRows+1 );
   1445 	assert( w.GetSize() >= numColumns+1 );
   1446 
   1447 	ChangeSize( numRows+1, numColumns+1, true );
   1448 
   1449 	// add row to L
   1450 	for ( i = 0; i < numRows - 1; i++ ) {
   1451 		sum = w[i];
   1452 		for ( j = 0; j < i; j++ ) {
   1453 			sum -= (*this)[numRows - 1][j] * (*this)[j][i];
   1454 		}
   1455 		(*this)[numRows - 1 ][i] = sum / (*this)[i][i];
   1456 	}
   1457 
   1458 	// add row to the permutation index
   1459 	if ( index != NULL ) {
   1460 		index[numRows - 1] = numRows - 1;
   1461 	}
   1462 
   1463 	// add column to U
   1464 	for ( i = 0; i < numRows; i++ ) {
   1465 		if ( index != NULL ) {
   1466 			sum = v[index[i]];
   1467 		} else {
   1468 			sum = v[i];
   1469 		}
   1470 		for ( j = 0; j < i; j++ ) {
   1471 			sum -= (*this)[i][j] * (*this)[j][numRows - 1];
   1472 		}
   1473 		(*this)[i][numRows - 1] = sum;
   1474 	}
   1475 
   1476 	return true;
   1477 }
   1478 
   1479 /*
   1480 ============
   1481 idMatX::LU_UpdateDecrement
   1482 
   1483   Updates the in-place LU factorization to obtain the factors for the matrix with row r and column r removed.
   1484   v and w should store the column and row of the original matrix respectively.
   1485   If index != NULL then u should store row index[r] of the original matrix. If index == NULL then u = w.
   1486 ============
   1487 */
   1488 bool idMatX::LU_UpdateDecrement( const idVecX &v, const idVecX &w, const idVecX &u, int r, int *index ) {
   1489 	int i, p;
   1490 	idVecX v1, w1;
   1491 
   1492 	assert( numRows == numColumns );
   1493 	assert( v.GetSize() >= numColumns );
   1494 	assert( w.GetSize() >= numRows );
   1495 	assert( r >= 0 && r < numRows && r < numColumns );
   1496 
   1497 	v1.SetData( numRows, VECX_ALLOCA( numRows ) );
   1498 	w1.SetData( numRows, VECX_ALLOCA( numRows ) );
   1499 
   1500 	if ( index != NULL ) {
   1501 
   1502 		// find the pivot row
   1503 		for ( p = i = 0; i < numRows; i++ ) {
   1504 			if ( index[i] == r ) {
   1505 				p = i;
   1506 				break;
   1507 			}
   1508 		}
   1509 
   1510 		// update the row and column to identity
   1511 		v1 = -v;
   1512 		w1 = -u;
   1513 
   1514 		if ( p != r ) {
   1515 			SwapValues( v1[index[r]], v1[index[p]] );
   1516 			SwapValues( index[r], index[p] );
   1517 		}
   1518 
   1519 		v1[r] += 1.0f;
   1520 		w1[r] = 0.0f;
   1521 
   1522 		if ( !LU_UpdateRowColumn( v1, w1, r, index ) ) {
   1523 			return false;
   1524 		}
   1525 
   1526 		if ( p != r ) {
   1527 
   1528 			if ( idMath::Fabs( u[p] ) < 1e-4f ) {
   1529 				// NOTE: an additional row interchange is required for numerical stability
   1530 			}
   1531 
   1532 			// move row index[r] of the original matrix to row index[p] of the original matrix
   1533 			v1.Zero();
   1534 			v1[index[p]] = 1.0f;
   1535 			w1 = u - w;
   1536 
   1537 			if ( !LU_UpdateRankOne( v1, w1, 1.0f, index ) ) {
   1538 				return false;
   1539 			}
   1540 		}
   1541 
   1542 		// remove the row from the permutation index
   1543 		for ( i = r; i < numRows - 1; i++ ) {
   1544 			index[i] = index[i+1];
   1545 		}
   1546 		for ( i = 0; i < numRows - 1; i++ ) {
   1547 			if ( index[i] > r ) {
   1548 				index[i]--;
   1549 			}
   1550 		}
   1551 
   1552 	} else {
   1553 
   1554 		v1 = -v;
   1555 		w1 = -w;
   1556 		v1[r] += 1.0f;
   1557 		w1[r] = 0.0f;
   1558 
   1559 		if ( !LU_UpdateRowColumn( v1, w1, r, index ) ) {
   1560 			return false;
   1561 		}
   1562 	}
   1563 
   1564 	// physically remove the row and column
   1565 	Update_Decrement( r );
   1566 
   1567 	return true;
   1568 }
   1569 
   1570 /*
   1571 ============
   1572 idMatX::LU_Solve
   1573 
   1574   Solve Ax = b with A factored in-place as: LU
   1575 ============
   1576 */
   1577 void idMatX::LU_Solve( idVecX &x, const idVecX &b, const int *index ) const {
   1578 	int i, j;
   1579 	double sum;
   1580 
   1581 	assert( x.GetSize() == numColumns && b.GetSize() == numRows );
   1582 
   1583 	// solve L
   1584 	for ( i = 0; i < numRows; i++ ) {
   1585 		if ( index != NULL ) {
   1586 			sum = b[index[i]];
   1587 		} else {
   1588 			sum = b[i];
   1589 		}
   1590 		for ( j = 0; j < i; j++ ) {
   1591 			sum -= (*this)[i][j] * x[j];
   1592 		}
   1593 		x[i] = sum;
   1594 	}
   1595 
   1596 	// solve U
   1597 	for ( i = numRows - 1; i >= 0; i-- ) {
   1598 		sum = x[i];
   1599 		for ( j = i + 1; j < numRows; j++ ) {
   1600 			sum -= (*this)[i][j] * x[j];
   1601 		}
   1602 		x[i] = sum / (*this)[i][i];
   1603 	}
   1604 }
   1605 
   1606 /*
   1607 ============
   1608 idMatX::LU_Inverse
   1609 
   1610   Calculates the inverse of the matrix which is factored in-place as LU
   1611 ============
   1612 */
   1613 void idMatX::LU_Inverse( idMatX &inv, const int *index ) const {
   1614 	int i, j;
   1615 	idVecX x, b;
   1616 
   1617 	assert( numRows == numColumns );
   1618 
   1619 	x.SetData( numRows, VECX_ALLOCA( numRows ) );
   1620 	b.SetData( numRows, VECX_ALLOCA( numRows ) );
   1621 	b.Zero();
   1622 	inv.SetSize( numRows, numColumns );
   1623 
   1624 	for ( i = 0; i < numRows; i++ ) {
   1625 
   1626 		b[i] = 1.0f;
   1627 		LU_Solve( x, b, index );
   1628 		for ( j = 0; j < numRows; j++ ) {
   1629 			inv[j][i] = x[j];
   1630 		}
   1631 		b[i] = 0.0f;
   1632 	}
   1633 }
   1634 
   1635 /*
   1636 ============
   1637 idMatX::LU_UnpackFactors
   1638 
   1639   Unpacks the in-place LU factorization.
   1640 ============
   1641 */
   1642 void idMatX::LU_UnpackFactors( idMatX &L, idMatX &U ) const {
   1643 	int i, j;
   1644 
   1645 	L.Zero( numRows, numColumns );
   1646 	U.Zero( numRows, numColumns );
   1647 	for ( i = 0; i < numRows; i++ ) {
   1648 		for ( j = 0; j < i; j++ ) {
   1649 			L[i][j] = (*this)[i][j];
   1650 		}
   1651 		L[i][i] = 1.0f;
   1652 		for ( j = i; j < numColumns; j++ ) {
   1653 			U[i][j] = (*this)[i][j];
   1654 		}
   1655 	}
   1656 }
   1657 
   1658 /*
   1659 ============
   1660 idMatX::LU_MultiplyFactors
   1661 
   1662   Multiplies the factors of the in-place LU factorization to form the original matrix.
   1663 ============
   1664 */
   1665 void idMatX::LU_MultiplyFactors( idMatX &m, const int *index ) const {
   1666 	int r, rp, i, j;
   1667 	double sum;
   1668 
   1669 	m.SetSize( numRows, numColumns );
   1670 
   1671 	for ( r = 0; r < numRows; r++ ) {
   1672 
   1673 		if ( index != NULL ) {
   1674 			rp = index[r];
   1675 		} else {
   1676 			rp = r;
   1677 		}
   1678 
   1679 		// calculate row of matrix
   1680 		for ( i = 0; i < numColumns; i++ ) {
   1681 			if ( i >= r ) {
   1682 				sum = (*this)[r][i];
   1683 			} else {
   1684 				sum = 0.0f;
   1685 			}
   1686 			for ( j = 0; j <= i && j < r; j++ ) {
   1687 				sum += (*this)[r][j] * (*this)[j][i];
   1688 			}
   1689 			m[rp][i] = sum;
   1690 		}
   1691 	}
   1692 }
   1693 
   1694 /*
   1695 ============
   1696 idMatX::QR_Factor
   1697 
   1698   in-place factorization: QR
   1699   Q is an orthogonal matrix represented as a product of Householder matrices stored in the lower triangle and c.
   1700   R is a triangular matrix stored in the upper triangle except for the diagonal elements which are stored in d.
   1701   The initial matrix has to be square.
   1702 ============
   1703 */
   1704 bool idMatX::QR_Factor( idVecX &c, idVecX &d ) {
   1705 	int i, j, k;
   1706 	double scale, s, t, sum;
   1707 	bool singular = false;
   1708 
   1709 	assert( numRows == numColumns );
   1710 	assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
   1711 
   1712 	for ( k = 0; k < numRows-1; k++ ) {
   1713 
   1714 		scale = 0.0f;
   1715 		for ( i = k; i < numRows; i++ ) {
   1716 			s = idMath::Fabs( (*this)[i][k] );
   1717 			if ( s > scale ) {
   1718 				scale = s;
   1719 			}
   1720 		}
   1721 		if ( scale == 0.0f ) {
   1722 			singular = true;
   1723 			c[k] = d[k] = 0.0f;
   1724 		} else {
   1725 
   1726 			s = 1.0f / scale;
   1727 			for ( i = k; i < numRows; i++ ) {
   1728 				(*this)[i][k] *= s;
   1729 			}
   1730 
   1731 			sum = 0.0f;
   1732 			for ( i = k; i < numRows; i++ ) {
   1733 				s = (*this)[i][k];
   1734 				sum += s * s;
   1735 			}
   1736 
   1737 			s = idMath::Sqrt( sum );
   1738 			if ( (*this)[k][k] < 0.0f ) {
   1739 				s = -s;
   1740 			}
   1741 			(*this)[k][k] += s;
   1742 			c[k] = s * (*this)[k][k];
   1743 			d[k] = -scale * s;
   1744 
   1745 			for ( j = k+1; j < numRows; j++ ) {
   1746 
   1747 				sum = 0.0f;
   1748 				for ( i = k; i < numRows; i++ ) {
   1749 					sum += (*this)[i][k] * (*this)[i][j];
   1750 				}
   1751 				t = sum / c[k];
   1752 				for ( i = k; i < numRows; i++ ) {
   1753 					(*this)[i][j] -= t * (*this)[i][k];
   1754 				}
   1755 			}
   1756 		}
   1757 	}
   1758 	d[numRows-1] = (*this)[ (numRows-1) ][ (numRows-1) ];
   1759 	if ( d[numRows-1] == 0.0f ) {
   1760 		singular = true;
   1761 	}
   1762 
   1763 	return !singular;
   1764 }
   1765 
   1766 /*
   1767 ============
   1768 idMatX::QR_Rotate
   1769 
   1770   Performs a Jacobi rotation on the rows i and i+1 of the unpacked QR factors.
   1771 ============
   1772 */
   1773 void idMatX::QR_Rotate( idMatX &R, int i, float a, float b ) {
   1774 	int j;
   1775 	float f, c, s, w, y;
   1776 
   1777 	if ( a == 0.0f ) {
   1778 		c = 0.0f;
   1779 		s = ( b >= 0.0f ) ? 1.0f : -1.0f;
   1780 	} else if ( idMath::Fabs( a ) > idMath::Fabs( b ) ) {
   1781 		f = b / a;
   1782 		c = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
   1783 		if ( a < 0.0f ) {
   1784 			c = -c;
   1785 		}
   1786 		s = f * c;
   1787 	} else {
   1788 		f = a / b;
   1789 		s = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
   1790 		if ( b < 0.0f ) {
   1791 			s = -s;
   1792 		}
   1793 		c = f * s;
   1794 	}
   1795 	for ( j = i; j < numRows; j++ ) {
   1796 		y = R[i][j];
   1797 		w = R[i+1][j];
   1798 		R[i][j] = c * y - s * w;
   1799 		R[i+1][j] = s * y + c * w;
   1800 	}
   1801 	for ( j = 0; j < numRows; j++ ) {
   1802 		y = (*this)[j][i];
   1803 		w = (*this)[j][i+1];
   1804 		(*this)[j][i] = c * y - s * w;
   1805 		(*this)[j][i+1] = s * y + c * w;
   1806 	}
   1807 }
   1808 
   1809 /*
   1810 ============
   1811 idMatX::QR_UpdateRankOne
   1812 
   1813   Updates the unpacked QR factorization to obtain the factors for the matrix: QR + alpha * v * w'
   1814 ============
   1815 */
   1816 bool idMatX::QR_UpdateRankOne( idMatX &R, const idVecX &v, const idVecX &w, float alpha ) {
   1817 	int i, k;
   1818 	float f;
   1819 	idVecX u;
   1820 
   1821 	assert( v.GetSize() >= numColumns );
   1822 	assert( w.GetSize() >= numRows );
   1823 
   1824 	u.SetData( v.GetSize(), VECX_ALLOCA( v.GetSize() ) );
   1825 	TransposeMultiply( u, v );
   1826 	u *= alpha;
   1827 
   1828 	for ( k = v.GetSize()-1; k > 0; k-- ) {
   1829 		if ( u[k] != 0.0f ) {
   1830 			break;
   1831 		}
   1832 	}
   1833 	for ( i = k-1; i >= 0; i-- ) {
   1834 		QR_Rotate( R, i, u[i], -u[i+1] );
   1835 		if ( u[i] == 0.0f ) {
   1836 			u[i] = idMath::Fabs( u[i+1] );
   1837 		} else if ( idMath::Fabs( u[i] ) > idMath::Fabs( u[i+1] ) ) {
   1838 			f = u[i+1] / u[i];
   1839 			u[i] = idMath::Fabs( u[i] ) * idMath::Sqrt( 1.0f + f * f );
   1840 		} else {
   1841 			f = u[i] / u[i+1];
   1842 			u[i] = idMath::Fabs( u[i+1] ) * idMath::Sqrt( 1.0f + f * f );
   1843 		}
   1844 	}
   1845 	for ( i = 0; i < v.GetSize(); i++ ) {
   1846 		R[0][i] += u[0] * w[i];
   1847 	}
   1848 	for ( i = 0; i < k; i++ ) {
   1849 		QR_Rotate( R, i, -R[i][i], R[i+1][i] );
   1850 	}
   1851 	return true;
   1852 }
   1853 
   1854 /*
   1855 ============
   1856 idMatX::QR_UpdateRowColumn
   1857 
   1858   Updates the unpacked QR factorization to obtain the factors for the matrix:
   1859 
   1860        [ 0  a  0 ]
   1861   QR + [ d  b  e ]
   1862        [ 0  c  0 ]
   1863 
   1864   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
   1865 ============
   1866 */
   1867 bool idMatX::QR_UpdateRowColumn( idMatX &R, const idVecX &v, const idVecX &w, int r ) {
   1868 	idVecX s;
   1869 
   1870 	assert( v.GetSize() >= numColumns );
   1871 	assert( w.GetSize() >= numRows );
   1872 	assert( r >= 0 && r < numRows && r < numColumns );
   1873 	assert( w[r] == 0.0f );
   1874 
   1875 	s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
   1876 	s.Zero();
   1877 	s[r] = 1.0f;
   1878 
   1879 	if ( !QR_UpdateRankOne( R, v, s, 1.0f ) ) {
   1880 		return false;
   1881 	}
   1882 	if ( !QR_UpdateRankOne( R, s, w, 1.0f ) ) {
   1883 		return false;
   1884 	}
   1885 	return true;
   1886 }
   1887 
   1888 /*
   1889 ============
   1890 idMatX::QR_UpdateIncrement
   1891 
   1892   Updates the unpacked QR factorization to obtain the factors for the matrix:
   1893 
   1894   [ A  a ]
   1895   [ c  b ]
   1896 
   1897   where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
   1898 ============
   1899 */
   1900 bool idMatX::QR_UpdateIncrement( idMatX &R, const idVecX &v, const idVecX &w ) {
   1901 	idVecX v2;
   1902 
   1903 	assert( numRows == numColumns );
   1904 	assert( v.GetSize() >= numRows+1 );
   1905 	assert( w.GetSize() >= numColumns+1 );
   1906 
   1907 	ChangeSize( numRows+1, numColumns+1, true );
   1908 	(*this)[numRows-1][numRows-1] = 1.0f;
   1909 
   1910 	R.ChangeSize( R.numRows+1, R.numColumns+1, true );
   1911 	R[R.numRows-1][R.numRows-1] = 1.0f;
   1912 
   1913 	v2.SetData( numRows, VECX_ALLOCA( numRows ) );
   1914 	v2 = v;
   1915 	v2[numRows-1] -= 1.0f;
   1916 
   1917 	return QR_UpdateRowColumn( R, v2, w, numRows-1 );
   1918 }
   1919 
   1920 /*
   1921 ============
   1922 idMatX::QR_UpdateDecrement
   1923 
   1924   Updates the unpacked QR factorization to obtain the factors for the matrix with row r and column r removed.
   1925   v and w should store the column and row of the original matrix respectively.
   1926 ============
   1927 */
   1928 bool idMatX::QR_UpdateDecrement( idMatX &R, const idVecX &v, const idVecX &w, int r ) {
   1929 	idVecX v1, w1;
   1930 
   1931 	assert( numRows == numColumns );
   1932 	assert( v.GetSize() >= numRows );
   1933 	assert( w.GetSize() >= numColumns );
   1934 	assert( r >= 0 && r < numRows && r < numColumns );
   1935 
   1936 	v1.SetData( numRows, VECX_ALLOCA( numRows ) );
   1937 	w1.SetData( numRows, VECX_ALLOCA( numRows ) );
   1938 
   1939 	// update the row and column to identity
   1940 	v1 = -v;
   1941 	w1 = -w;
   1942 	v1[r] += 1.0f;
   1943 	w1[r] = 0.0f;
   1944 
   1945 	if ( !QR_UpdateRowColumn( R, v1, w1, r ) ) {
   1946 		return false;
   1947 	}
   1948 
   1949 	// physically remove the row and column
   1950 	Update_Decrement( r );
   1951 	R.Update_Decrement( r );
   1952 
   1953 	return true;
   1954 }
   1955 
   1956 /*
   1957 ============
   1958 idMatX::QR_Solve
   1959 
   1960   Solve Ax = b with A factored in-place as: QR
   1961 ============
   1962 */
   1963 void idMatX::QR_Solve( idVecX &x, const idVecX &b, const idVecX &c, const idVecX &d ) const {
   1964 	int i, j;
   1965 	double sum, t;
   1966 
   1967 	assert( numRows == numColumns );
   1968 	assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
   1969 	assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
   1970 
   1971 	for ( i = 0; i < numRows; i++ ) {
   1972 		x[i] = b[i];
   1973 	}
   1974 
   1975 	// multiply b with transpose of Q
   1976 	for ( i = 0; i < numRows-1; i++ ) {
   1977 
   1978 		sum = 0.0f;
   1979 		for ( j = i; j < numRows; j++ ) {
   1980 			sum += (*this)[j][i] * x[j];
   1981 		}
   1982 		t = sum / c[i];
   1983 		for ( j = i; j < numRows; j++ ) {
   1984 			x[j] -= t * (*this)[j][i];
   1985 		}
   1986 	}
   1987 
   1988 	// backsubstitution with R
   1989 	for ( i = numRows-1; i >= 0; i-- ) {
   1990 
   1991 		sum = x[i];
   1992 		for ( j = i + 1; j < numRows; j++ ) {
   1993 			sum -= (*this)[i][j] * x[j];
   1994 		}
   1995 		x[i] = sum / d[i];
   1996 	}
   1997 }
   1998 
   1999 /*
   2000 ============
   2001 idMatX::QR_Solve
   2002 
   2003   Solve Ax = b with A factored as: QR
   2004 ============
   2005 */
   2006 void idMatX::QR_Solve( idVecX &x, const idVecX &b, const idMatX &R ) const {
   2007 	int i, j;
   2008 	double sum;
   2009 
   2010 	assert( numRows == numColumns );
   2011 
   2012 	// multiply b with transpose of Q
   2013 	TransposeMultiply( x, b );
   2014 
   2015 	// backsubstitution with R
   2016 	for ( i = numRows-1; i >= 0; i-- ) {
   2017 
   2018 		sum = x[i];
   2019 		for ( j = i + 1; j < numRows; j++ ) {
   2020 			sum -= R[i][j] * x[j];
   2021 		}
   2022 		x[i] = sum / R[i][i];
   2023 	}
   2024 }
   2025 
   2026 /*
   2027 ============
   2028 idMatX::QR_Inverse
   2029 
   2030   Calculates the inverse of the matrix which is factored in-place as: QR
   2031 ============
   2032 */
   2033 void idMatX::QR_Inverse( idMatX &inv, const idVecX &c, const idVecX &d ) const {
   2034 	int i, j;
   2035 	idVecX x, b;
   2036 
   2037 	assert( numRows == numColumns );
   2038 
   2039 	x.SetData( numRows, VECX_ALLOCA( numRows ) );
   2040 	b.SetData( numRows, VECX_ALLOCA( numRows ) );
   2041 	b.Zero();
   2042 	inv.SetSize( numRows, numColumns );
   2043 
   2044 	for ( i = 0; i < numRows; i++ ) {
   2045 
   2046 		b[i] = 1.0f;
   2047 		QR_Solve( x, b, c, d );
   2048 		for ( j = 0; j < numRows; j++ ) {
   2049 			inv[j][i] = x[j];
   2050 		}
   2051 		b[i] = 0.0f;
   2052 	}
   2053 }
   2054 
   2055 /*
   2056 ============
   2057 idMatX::QR_UnpackFactors
   2058 
   2059   Unpacks the in-place QR factorization.
   2060 ============
   2061 */
   2062 void idMatX::QR_UnpackFactors( idMatX &Q, idMatX &R, const idVecX &c, const idVecX &d ) const {
   2063 	int i, j, k;
   2064 	double sum;
   2065 
   2066 	Q.Identity( numRows, numColumns );
   2067 	for ( i = 0; i < numColumns-1; i++ ) {
   2068 		if ( c[i] == 0.0f ) {
   2069 			continue;
   2070 		}
   2071 		for ( j = 0; j < numRows; j++ ) {
   2072 			sum = 0.0f;
   2073 			for ( k = i; k < numColumns; k++ ) {
   2074 				sum += (*this)[k][i] * Q[j][k];
   2075 			}
   2076 			sum /= c[i];
   2077 			for ( k = i; k < numColumns; k++ ) {
   2078 				Q[j][k] -= sum * (*this)[k][i];
   2079 			}
   2080 		}
   2081 	}
   2082 
   2083 	R.Zero( numRows, numColumns );
   2084 	for ( i = 0; i < numRows; i++ ) {
   2085 		R[i][i] = d[i];
   2086 		for ( j = i+1; j < numColumns; j++ ) {
   2087 			R[i][j] = (*this)[i][j];
   2088 		}
   2089 	}
   2090 }
   2091 
   2092 /*
   2093 ============
   2094 idMatX::QR_MultiplyFactors
   2095 
   2096   Multiplies the factors of the in-place QR factorization to form the original matrix.
   2097 ============
   2098 */
   2099 void idMatX::QR_MultiplyFactors( idMatX &m, const idVecX &c, const idVecX &d ) const {
   2100 	int i, j, k;
   2101 	double sum;
   2102 	idMatX Q;
   2103 
   2104 	Q.Identity( numRows, numColumns );
   2105 	for ( i = 0; i < numColumns-1; i++ ) {
   2106 		if ( c[i] == 0.0f ) {
   2107 			continue;
   2108 		}
   2109 		for ( j = 0; j < numRows; j++ ) {
   2110 			sum = 0.0f;
   2111 			for ( k = i; k < numColumns; k++ ) {
   2112 				sum += (*this)[k][i] * Q[j][k];
   2113 			}
   2114 			sum /= c[i];
   2115 			for ( k = i; k < numColumns; k++ ) {
   2116 				Q[j][k] -= sum * (*this)[k][i];
   2117 			}
   2118 		}
   2119 	}
   2120 
   2121 	for ( i = 0; i < numRows; i++ ) {
   2122 		for ( j = 0; j < numColumns; j++ ) {
   2123 			sum = Q[i][j] * d[i];
   2124 			for ( k = 0; k < i; k++ ) {
   2125 				sum += Q[i][k] * (*this)[j][k];
   2126 			}
   2127 			m[i][j] = sum;
   2128 		}
   2129 	}
   2130 }
   2131 
   2132 /*
   2133 ============
   2134 idMatX::Pythag
   2135 
   2136   Computes (a^2 + b^2)^1/2 without underflow or overflow.
   2137 ============
   2138 */
   2139 float idMatX::Pythag( float a, float b ) const {
   2140 	double at, bt, ct;
   2141 
   2142 	at = idMath::Fabs( a );
   2143 	bt = idMath::Fabs( b );
   2144 	if ( at > bt ) {
   2145 		ct = bt / at;
   2146 		return at * idMath::Sqrt( 1.0f + ct * ct );
   2147 	} else {
   2148 		if ( bt ) {
   2149 			ct = at / bt;
   2150 			return bt * idMath::Sqrt( 1.0f + ct * ct );
   2151 		} else {
   2152 			return 0.0f;
   2153 		}
   2154 	}
   2155 }
   2156 
   2157 /*
   2158 ============
   2159 idMatX::SVD_BiDiag
   2160 ============
   2161 */
   2162 void idMatX::SVD_BiDiag( idVecX &w, idVecX &rv1, float &anorm ) {
   2163 	int i, j, k, l;
   2164 	double f, h, r, g, s, scale;
   2165 
   2166 	anorm = 0.0f;
   2167 	g = s = scale = 0.0f;
   2168 	for ( i = 0; i < numColumns; i++ ) {
   2169 		l = i + 1;
   2170 		rv1[i] = scale * g;
   2171 		g = s = scale = 0.0f;
   2172 		if ( i < numRows ) {
   2173 			for ( k = i; k < numRows; k++ ) {
   2174 				scale += idMath::Fabs( (*this)[k][i] );
   2175 			}
   2176 			if ( scale ) {
   2177 				for ( k = i; k < numRows; k++ ) {
   2178 					(*this)[k][i] /= scale;
   2179 					s += (*this)[k][i] * (*this)[k][i];
   2180 				}
   2181 				f = (*this)[i][i];
   2182 				g = idMath::Sqrt( s );
   2183 				if ( f >= 0.0f ) {
   2184 					g = -g;
   2185 				}
   2186 				h = f * g - s;
   2187 				(*this)[i][i] = f - g;
   2188 				if ( i != (numColumns-1) ) {
   2189 					for ( j = l; j < numColumns; j++ ) {
   2190 						for ( s = 0.0f, k = i; k < numRows; k++ ) {
   2191 							s += (*this)[k][i] * (*this)[k][j];
   2192 						}
   2193 						f = s / h;
   2194 						for ( k = i; k < numRows; k++ ) {
   2195 							(*this)[k][j] += f * (*this)[k][i];
   2196 						}
   2197 					}
   2198 				}
   2199 				for ( k = i; k < numRows; k++ ) {
   2200 					(*this)[k][i] *= scale;
   2201 				}
   2202 			}
   2203 		}
   2204 		w[i] = scale * g;
   2205 		g = s = scale = 0.0f;
   2206 		if ( i < numRows && i != (numColumns-1) ) {
   2207 			for ( k = l; k < numColumns; k++ ) {
   2208 				scale += idMath::Fabs( (*this)[i][k] );
   2209 			}
   2210 			if ( scale ) {
   2211 				for ( k = l; k < numColumns; k++ ) {
   2212 					(*this)[i][k] /= scale;
   2213 					s += (*this)[i][k] * (*this)[i][k];
   2214 				}
   2215 				f = (*this)[i][l];
   2216 				g = idMath::Sqrt( s );
   2217 				if ( f >= 0.0f ) {
   2218 					g = -g;
   2219 				}
   2220 				h = 1.0f / ( f * g - s );
   2221 				(*this)[i][l] = f - g;
   2222 				for ( k = l; k < numColumns; k++ ) {
   2223 					rv1[k] = (*this)[i][k] * h;
   2224 				}
   2225 				if ( i != (numRows-1) ) {
   2226 					for ( j = l; j < numRows; j++ ) {
   2227 						for ( s = 0.0f, k = l; k < numColumns; k++ ) {
   2228 							s += (*this)[j][k] * (*this)[i][k];
   2229 						}
   2230 						for ( k = l; k < numColumns; k++ ) {
   2231 							(*this)[j][k] += s * rv1[k];
   2232 						}
   2233 					}
   2234 				}
   2235 				for ( k = l; k < numColumns; k++ ) {
   2236 					(*this)[i][k] *= scale;
   2237 				}
   2238 			}
   2239 		}
   2240 		r = idMath::Fabs( w[i] ) + idMath::Fabs( rv1[i] );
   2241 		if ( r > anorm ) {
   2242 			anorm = r;
   2243 		}
   2244 	}
   2245 }
   2246 
   2247 /*
   2248 ============
   2249 idMatX::SVD_InitialWV
   2250 ============
   2251 */
   2252 void idMatX::SVD_InitialWV( idVecX &w, idMatX &V, idVecX &rv1 ) {
   2253 	int i, j, k, l;
   2254 	double f, g, s;
   2255 
   2256 	g = 0.0f;
   2257 	for ( i = (numColumns-1); i >= 0; i-- ) {
   2258 		l = i + 1;
   2259 		if ( i < ( numColumns - 1 ) ) {
   2260 			if ( g ) {
   2261 				for ( j = l; j < numColumns; j++ ) {
   2262 					V[j][i] = ((*this)[i][j] / (*this)[i][l]) / g;
   2263 				}
   2264 				// double division to reduce underflow
   2265 				for ( j = l; j < numColumns; j++ ) {
   2266 					for ( s = 0.0f, k = l; k < numColumns; k++ ) {
   2267 						s += (*this)[i][k] * V[k][j];
   2268 					}
   2269 					for ( k = l; k < numColumns; k++ ) {
   2270 						V[k][j] += s * V[k][i];
   2271 					}
   2272 				}
   2273 			}
   2274 			for ( j = l; j < numColumns; j++ ) {
   2275 				V[i][j] = V[j][i] = 0.0f;
   2276 			}
   2277 		}
   2278 		V[i][i] = 1.0f;
   2279 		g = rv1[i];
   2280 	}
   2281 	for ( i = numColumns - 1 ; i >= 0; i-- ) {
   2282 		l = i + 1;
   2283 		g = w[i];
   2284 		if ( i < (numColumns-1) ) {
   2285 			for ( j = l; j < numColumns; j++ ) {
   2286 				(*this)[i][j] = 0.0f;
   2287 			}
   2288 		}
   2289 		if ( g ) {
   2290 			g = 1.0f / g;
   2291 			if ( i != (numColumns-1) ) {
   2292 				for ( j = l; j < numColumns; j++ ) {
   2293 					for ( s = 0.0f, k = l; k < numRows; k++ ) {
   2294 						s += (*this)[k][i] * (*this)[k][j];
   2295 					}
   2296 					f = (s / (*this)[i][i]) * g;
   2297 					for ( k = i; k < numRows; k++ ) {
   2298 						(*this)[k][j] += f * (*this)[k][i];
   2299 					}
   2300 				}
   2301 			}
   2302 			for ( j = i; j < numRows; j++ ) {
   2303 				(*this)[j][i] *= g;
   2304 			}
   2305 		}
   2306 		else {
   2307 			for ( j = i; j < numRows; j++ ) {
   2308 				(*this)[j][i] = 0.0f;
   2309 			}
   2310 		}
   2311 		(*this)[i][i] += 1.0f;
   2312 	}
   2313 }
   2314 
   2315 /*
   2316 ============
   2317 idMatX::SVD_Factor
   2318 
   2319   in-place factorization: U * Diag(w) * V.Transpose()
   2320   known as the Singular Value Decomposition.
   2321   U is a column-orthogonal matrix which overwrites the original matrix.
   2322   w is a diagonal matrix with all elements >= 0 which are the singular values.
   2323   V is the transpose of an orthogonal matrix.
   2324 ============
   2325 */
   2326 bool idMatX::SVD_Factor( idVecX &w, idMatX &V ) {
   2327 	int flag, i, its, j, jj, k, l, nm;
   2328 	double c, f, h, s, x, y, z, r, g = 0.0f;
   2329 	float anorm = 0.0f;
   2330 	idVecX rv1;
   2331 
   2332 	if ( numRows < numColumns ) {
   2333 		return false;
   2334 	}
   2335 
   2336 	rv1.SetData( numColumns, VECX_ALLOCA( numColumns ) );
   2337 	rv1.Zero();
   2338 	w.Zero( numColumns );
   2339 	V.Zero( numColumns, numColumns );
   2340 
   2341 	SVD_BiDiag( w, rv1, anorm );
   2342 	SVD_InitialWV( w, V, rv1 );
   2343 
   2344 	for ( k = numColumns - 1; k >= 0; k-- ) {
   2345 		for ( its = 1; its <= 30; its++ ) {
   2346 			flag = 1;
   2347 			nm = 0;
   2348 			for ( l = k; l >= 0; l-- ) {
   2349 				nm = l - 1;
   2350 				if ( ( idMath::Fabs( rv1[l] ) + anorm ) == anorm /* idMath::Fabs( rv1[l] ) < idMath::FLT_EPSILON */ ) {
   2351 					flag = 0;
   2352 					break;
   2353 				}
   2354 				if ( ( idMath::Fabs( w[nm] ) + anorm ) == anorm /* idMath::Fabs( w[nm] ) < idMath::FLT_EPSILON */ ) {
   2355 					break;
   2356 				}
   2357 			}
   2358 			if ( flag ) {
   2359 				c = 0.0f;
   2360 				s = 1.0f;
   2361 				for ( i = l; i <= k; i++ ) {
   2362 					f = s * rv1[i];
   2363 
   2364 					if ( ( idMath::Fabs( f ) + anorm ) != anorm /* idMath::Fabs( f ) > idMath::FLT_EPSILON */ ) {
   2365 						g = w[i];
   2366 						h = Pythag( f, g );
   2367 						w[i] = h;
   2368 						h = 1.0f / h;
   2369 						c = g * h;
   2370 						s = -f * h;
   2371 						for ( j = 0; j < numRows; j++ ) {
   2372 							y = (*this)[j][nm];
   2373 							z = (*this)[j][i];
   2374 							(*this)[j][nm] = y * c + z * s;
   2375 							(*this)[j][i] = z * c - y * s;
   2376 						}
   2377 					}
   2378 				}
   2379 			}
   2380 			z = w[k];
   2381 			if ( l == k ) {
   2382 				if ( z < 0.0f ) {
   2383 					w[k] = -z;
   2384 					for ( j = 0; j < numColumns; j++ ) {
   2385 						V[j][k] = -V[j][k];
   2386 					}
   2387 				}
   2388 				break;
   2389 			}
   2390 			if ( its == 30 ) {
   2391 				return false;		// no convergence
   2392 			}
   2393 			x = w[l];
   2394 			nm = k - 1;
   2395 			y = w[nm];
   2396 			g = rv1[nm];
   2397 			h = rv1[k];
   2398 			f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0f * h * y );
   2399 			g = Pythag( f, 1.0f );
   2400 			r = ( f >= 0.0f ? g : - g );
   2401 			f= ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + r ) ) - h ) ) / x;
   2402 			c = s = 1.0f;
   2403 			for ( j = l; j <= nm; j++ ) {
   2404 				i = j + 1;
   2405 				g = rv1[i];
   2406 				y = w[i];
   2407 				h = s * g;
   2408 				g = c * g;
   2409 				z = Pythag( f, h );
   2410 				rv1[j] = z;
   2411 				c = f / z;
   2412 				s = h / z;
   2413 				f = x * c + g * s;
   2414 				g = g * c - x * s;
   2415 				h = y * s;
   2416 				y = y * c;
   2417 				for ( jj = 0; jj < numColumns; jj++ ) {
   2418 					x = V[jj][j];
   2419 					z = V[jj][i];
   2420 					V[jj][j] = x * c + z * s;
   2421 					V[jj][i] = z * c - x * s;
   2422 				}
   2423 				z = Pythag( f, h );
   2424 				w[j] = z;
   2425 				if ( z ) {
   2426 					z = 1.0f / z;
   2427 					c = f * z;
   2428 					s = h * z;
   2429 				}
   2430 				f = ( c * g ) + ( s * y );
   2431 				x = ( c * y ) - ( s * g );
   2432 				for ( jj = 0; jj < numRows; jj++ ) {
   2433 					y = (*this)[jj][j];
   2434 					z = (*this)[jj][i];
   2435 					(*this)[jj][j] = y * c + z * s;
   2436 					(*this)[jj][i] = z * c - y * s;
   2437 				}
   2438 			}
   2439 			rv1[l] = 0.0f;
   2440 			rv1[k] = f;
   2441 			w[k] = x;
   2442 		}
   2443 	}
   2444 	return true;
   2445 }
   2446 
   2447 /*
   2448 ============
   2449 idMatX::SVD_Solve
   2450 
   2451   Solve Ax = b with A factored as: U * Diag(w) * V.Transpose()
   2452 ============
   2453 */
   2454 void idMatX::SVD_Solve( idVecX &x, const idVecX &b, const idVecX &w, const idMatX &V ) const {
   2455 	int i, j;
   2456 	double sum;
   2457 	idVecX tmp;
   2458 
   2459 	assert( x.GetSize() >= numColumns );
   2460 	assert( b.GetSize() >= numColumns );
   2461 	assert( w.GetSize() == numColumns );
   2462 	assert( V.GetNumRows() == numColumns && V.GetNumColumns() == numColumns );
   2463 
   2464 	tmp.SetData( numColumns, VECX_ALLOCA( numColumns ) );
   2465 
   2466 	for ( i = 0; i < numColumns; i++ ) {
   2467 		sum = 0.0f;
   2468 		if ( w[i] >= idMath::FLT_EPSILON ) {
   2469 			for ( j = 0; j < numRows; j++ ) {
   2470 				sum += (*this)[j][i] * b[j];
   2471 			}
   2472 			sum /= w[i];
   2473 		}
   2474 		tmp[i] = sum;
   2475 	}
   2476 	for ( i = 0; i < numColumns; i++ ) {
   2477 		sum = 0.0f;
   2478 		for ( j = 0; j < numColumns; j++ ) {
   2479 			sum += V[i][j] * tmp[j];
   2480 		}
   2481 		x[i] = sum;
   2482 	}
   2483 }
   2484 
   2485 /*
   2486 ============
   2487 idMatX::SVD_Inverse
   2488 
   2489   Calculates the inverse of the matrix which is factored in-place as: U * Diag(w) * V.Transpose()
   2490 ============
   2491 */
   2492 void idMatX::SVD_Inverse( idMatX &inv, const idVecX &w, const idMatX &V ) const {
   2493 	int i, j, k;
   2494 	double wi, sum;
   2495 	idMatX V2;
   2496 
   2497 	assert( numRows == numColumns );
   2498 
   2499 	V2 = V;
   2500 
   2501 	// V * [diag(1/w[i])]
   2502 	for ( i = 0; i < numRows; i++ ) {
   2503 		wi = w[i];
   2504 		wi = ( wi < idMath::FLT_EPSILON ) ? 0.0f : 1.0f / wi;
   2505 		for ( j = 0; j < numColumns; j++ ) {
   2506 			V2[j][i] *= wi;
   2507 		}
   2508 	}
   2509 
   2510 	// V * [diag(1/w[i])] * Ut
   2511 	for ( i = 0; i < numRows; i++ ) {
   2512 		for ( j = 0; j < numColumns; j++ ) {
   2513 			sum = V2[i][0] * (*this)[j][0];
   2514 			for ( k = 1; k < numColumns; k++ ) {
   2515 				sum += V2[i][k] * (*this)[j][k];
   2516 			}
   2517 			inv[i][j] = sum;
   2518 		}
   2519 	}
   2520 }
   2521 
   2522 /*
   2523 ============
   2524 idMatX::SVD_MultiplyFactors
   2525 
   2526   Multiplies the factors of the in-place SVD factorization to form the original matrix.
   2527 ============
   2528 */
   2529 void idMatX::SVD_MultiplyFactors( idMatX &m, const idVecX &w, const idMatX &V ) const {
   2530 	int r, i, j;
   2531 	double sum;
   2532 
   2533 	m.SetSize( numRows, V.GetNumRows() );
   2534 
   2535 	for ( r = 0; r < numRows; r++ ) {
   2536 		// calculate row of matrix
   2537 		if ( w[r] >= idMath::FLT_EPSILON ) {
   2538 			for ( i = 0; i < V.GetNumRows(); i++ ) {
   2539 				sum = 0.0f;
   2540 				for ( j = 0; j < numColumns; j++ ) {
   2541 					sum += (*this)[r][j] * V[i][j];
   2542 				}
   2543 				m[r][i] = sum * w[r];
   2544 			}
   2545 		} else {
   2546 			for ( i = 0; i < V.GetNumRows(); i++ ) {
   2547 				m[r][i] = 0.0f;
   2548 			}
   2549 		}
   2550 	}
   2551 }
   2552 
   2553 /*
   2554 ============
   2555 idMatX::Cholesky_Factor
   2556 
   2557   in-place Cholesky factorization: LL'
   2558   L is a triangular matrix stored in the lower triangle.
   2559   The upper triangle is not cleared.
   2560   The initial matrix has to be symmetric positive definite.
   2561 ============
   2562 */
   2563 bool idMatX::Cholesky_Factor() {
   2564 	int i, j, k;
   2565 	float *invSqrt;
   2566 	double sum;
   2567 
   2568 	assert( numRows == numColumns );
   2569 
   2570 	invSqrt = (float *) _alloca16( numRows * sizeof( float ) );
   2571 
   2572 	for ( i = 0; i < numRows; i++ ) {
   2573 
   2574 		for ( j = 0; j < i; j++ ) {
   2575 
   2576 			sum = (*this)[i][j];
   2577 			for ( k = 0; k < j; k++ ) {
   2578 				sum -= (*this)[i][k] * (*this)[j][k];
   2579 			}
   2580 			(*this)[i][j] = sum * invSqrt[j];
   2581 		}
   2582 
   2583 		sum = (*this)[i][i];
   2584 		for ( k = 0; k < i; k++ ) {
   2585 			sum -= (*this)[i][k] * (*this)[i][k];
   2586 		}
   2587 
   2588 		if ( sum <= 0.0f ) {
   2589 			return false;
   2590 		}
   2591 
   2592 		invSqrt[i] = idMath::InvSqrt( sum );
   2593 		(*this)[i][i] = invSqrt[i] * sum;
   2594 	}
   2595 	return true;
   2596 }
   2597 
   2598 /*
   2599 ============
   2600 idMatX::Cholesky_UpdateRankOne
   2601 
   2602   Updates the in-place Cholesky factorization to obtain the factors for the matrix: LL' + alpha * v * v'
   2603   If offset > 0 only the lower right corner starting at (offset, offset) is updated.
   2604 ============
   2605 */
   2606 bool idMatX::Cholesky_UpdateRankOne( const idVecX &v, float alpha, int offset ) {
   2607 	int i, j;
   2608 	float *y;
   2609 	double diag, invDiag, diagSqr, newDiag, newDiagSqr, beta, p, d;
   2610 
   2611 	assert( numRows == numColumns );
   2612 	assert( v.GetSize() >= numRows );
   2613 	assert( offset >= 0 && offset < numRows );
   2614 
   2615 	y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
   2616 	memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
   2617 
   2618 	for ( i = offset; i < numColumns; i++ ) {
   2619 		p = y[i];
   2620 		diag = (*this)[i][i];
   2621 		invDiag = 1.0f / diag;
   2622 		diagSqr = diag * diag;
   2623 		newDiagSqr = diagSqr + alpha * p * p;
   2624 
   2625 		if ( newDiagSqr <= 0.0f ) {
   2626 			return false;
   2627 		}
   2628 
   2629 		(*this)[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
   2630 
   2631 		alpha /= newDiagSqr;
   2632 		beta = p * alpha;
   2633 		alpha *= diagSqr;
   2634 
   2635 		for ( j = i+1; j < numRows; j++ ) {
   2636 
   2637 			d = (*this)[j][i] * invDiag;
   2638 
   2639 			y[j] -= p * d;
   2640 			d += beta * y[j];
   2641 
   2642 			(*this)[j][i] = d * newDiag;
   2643 		}
   2644 	}
   2645 	return true;
   2646 }
   2647 
   2648 /*
   2649 ============
   2650 idMatX::Cholesky_UpdateRowColumn
   2651 
   2652   Updates the in-place Cholesky factorization to obtain the factors for the matrix:
   2653 
   2654         [ 0  a  0 ]
   2655   LL' + [ a  b  c ]
   2656         [ 0  c  0 ]
   2657 
   2658   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
   2659 ============
   2660 */
   2661 bool idMatX::Cholesky_UpdateRowColumn( const idVecX &v, int r ) {
   2662 	int i, j;
   2663 	double sum;
   2664 	float *original, *y;
   2665 	idVecX addSub;
   2666 
   2667 	assert( numRows == numColumns );
   2668 	assert( v.GetSize() >= numRows );
   2669 	assert( r >= 0 && r < numRows );
   2670 
   2671 	addSub.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
   2672 
   2673 	if ( r == 0 ) {
   2674 
   2675 		if ( numColumns == 1 ) {
   2676 			double v0 = v[0];
   2677 			sum = (*this)[0][0];
   2678 			sum = sum * sum; 
   2679 			sum = sum + v0; 
   2680 			if ( sum <= 0.0f ) {
   2681 				return false;
   2682 			}
   2683 			(*this)[0][0] = idMath::Sqrt( sum );
   2684 			return true;
   2685 		}
   2686 		for ( i = 0; i < numColumns; i++ ) {
   2687 			addSub[i] = v[i];
   2688 		}
   2689 
   2690 	} else {
   2691 
   2692 		original = (float *) _alloca16( numColumns * sizeof( float ) );
   2693 		y = (float *) _alloca16( numColumns * sizeof( float ) );
   2694 
   2695 		// calculate original row/column of matrix
   2696 		for ( i = 0; i < numRows; i++ ) {
   2697 			sum = 0.0f;
   2698 			for ( j = 0; j <= i; j++ ) {
   2699 				sum += (*this)[r][j] * (*this)[i][j];
   2700 			}
   2701 			original[i] = sum;
   2702 		}
   2703 
   2704 		// solve for y in L * y = original + v
   2705 		for ( i = 0; i < r; i++ ) {
   2706 			sum = original[i] + v[i];
   2707 			for ( j = 0; j < i; j++ ) {
   2708 				sum -= (*this)[r][j] * (*this)[i][j];
   2709 			}
   2710 			(*this)[r][i] = sum / (*this)[i][i];
   2711 		}
   2712 
   2713 		// if the last row/column of the matrix is updated
   2714 		if ( r == numColumns - 1 ) {
   2715 			// only calculate new diagonal
   2716 			sum = original[r] + v[r];
   2717 			for ( j = 0; j < r; j++) {
   2718 				sum -= (*this)[r][j] * (*this)[r][j];
   2719 			}
   2720 			if ( sum <= 0.0f ) {
   2721 				return false;
   2722 			}
   2723 			(*this)[r][r] = idMath::Sqrt( sum );
   2724 			return true;
   2725 		}
   2726 
   2727 		// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
   2728 		for ( i = r; i < numColumns; i++ ) {
   2729 			sum = 0.0f;
   2730 			for ( j = 0; j <= r; j++ ) {
   2731 				sum += (*this)[r][j] * (*this)[i][j];
   2732 			}
   2733 			addSub[i] = v[i] - ( sum - original[i] );
   2734 		}
   2735 	}
   2736 
   2737 	// add row/column to the lower right sub matrix starting at (r, r)
   2738 
   2739 #if 0
   2740 
   2741 	idVecX v1, v2;
   2742 	double d;
   2743 
   2744 	v1.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
   2745 	v2.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
   2746 
   2747 	d = idMath::SQRT_1OVER2;
   2748 	v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
   2749 	v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
   2750 	for ( i = r+1; i < numColumns; i++ ) {
   2751 		v1[i] = v2[i] = addSub[i] * d;
   2752 	}
   2753 
   2754 	// update
   2755 	if ( !Cholesky_UpdateRankOne( v1, 1.0f, r ) ) {
   2756 		return false;
   2757 	}
   2758 	// downdate
   2759 	if ( !Cholesky_UpdateRankOne( v2, -1.0f, r ) ) {
   2760 		return false;
   2761 	}
   2762 
   2763 #else
   2764 
   2765 	float *v1, *v2;
   2766 	double diag, invDiag, diagSqr, newDiag, newDiagSqr;
   2767 	double alpha1, alpha2, beta1, beta2, p1, p2, d;
   2768 
   2769 	v1 = (float *) _alloca16( numColumns * sizeof( float ) );
   2770 	v2 = (float *) _alloca16( numColumns * sizeof( float ) );
   2771 
   2772 	d = idMath::SQRT_1OVER2;
   2773 	v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
   2774 	v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
   2775 	for ( i = r+1; i < numColumns; i++ ) {
   2776 		v1[i] = v2[i] = addSub[i] * d;
   2777 	}
   2778 
   2779 	alpha1 = 1.0f;
   2780 	alpha2 = -1.0f;
   2781 
   2782 	// simultaneous update/downdate of the sub matrix starting at (r, r)
   2783 	for ( i = r; i < numColumns; i++ ) {
   2784 		p1 = v1[i];
   2785 		diag = (*this)[i][i];
   2786 		invDiag = 1.0f / diag;
   2787 		diagSqr = diag * diag;
   2788 		newDiagSqr = diagSqr + alpha1 * p1 * p1;
   2789 
   2790 		if ( newDiagSqr <= 0.0f ) {
   2791 			return false;
   2792 		}
   2793 
   2794 		alpha1 /= newDiagSqr;
   2795 		beta1 = p1 * alpha1;
   2796 		alpha1 *= diagSqr;
   2797 
   2798 		p2 = v2[i];
   2799 		diagSqr = newDiagSqr;
   2800 		newDiagSqr = diagSqr + alpha2 * p2 * p2;
   2801 
   2802 		if ( newDiagSqr <= 0.0f ) {
   2803 			return false;
   2804 		}
   2805 
   2806 		(*this)[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
   2807 
   2808 		alpha2 /= newDiagSqr;
   2809 		beta2 = p2 * alpha2;
   2810 		alpha2 *= diagSqr;
   2811 
   2812 		for ( j = i+1; j < numRows; j++ ) {
   2813 
   2814 			d = (*this)[j][i] * invDiag;
   2815 
   2816 			v1[j] -= p1 * d;
   2817 			d += beta1 * v1[j];
   2818 
   2819 			v2[j] -= p2 * d;
   2820 			d += beta2 * v2[j];
   2821 
   2822 			(*this)[j][i] = d * newDiag;
   2823 		}
   2824 	}
   2825 
   2826 #endif
   2827 
   2828 	return true;
   2829 }
   2830 
   2831 /*
   2832 ============
   2833 idMatX::Cholesky_UpdateIncrement
   2834 
   2835   Updates the in-place Cholesky factorization to obtain the factors for the matrix:
   2836 
   2837   [ A  a ]
   2838   [ a  b ]
   2839 
   2840   where: a = v[0,numRows-1], b = v[numRows]
   2841 ============
   2842 */
   2843 bool idMatX::Cholesky_UpdateIncrement( const idVecX &v ) {
   2844 	int i, j;
   2845 	float *x;
   2846 	double sum;
   2847 
   2848 	assert( numRows == numColumns );
   2849 	assert( v.GetSize() >= numRows+1 );
   2850 
   2851 	ChangeSize( numRows+1, numColumns+1, false );
   2852 
   2853 	x = (float *) _alloca16( numRows * sizeof( float ) );
   2854 
   2855 	// solve for x in L * x = v
   2856 	for ( i = 0; i < numRows - 1; i++ ) {
   2857 		sum = v[i];
   2858 		for ( j = 0; j < i; j++ ) {
   2859 			sum -= (*this)[i][j] * x[j];
   2860 		}
   2861 		x[i] = sum / (*this)[i][i];
   2862 	}
   2863 
   2864 	// calculate new row of L and calculate the square of the diagonal entry
   2865 	sum = v[numRows - 1];
   2866 	for ( i = 0; i < numRows - 1; i++ ) {
   2867 		(*this)[numRows - 1][i] = x[i];
   2868 		sum -= x[i] * x[i];
   2869 	}
   2870 
   2871 	if ( sum <= 0.0f ) {
   2872 		return false;
   2873 	}
   2874 
   2875 	// store the diagonal entry
   2876 	(*this)[numRows - 1][numRows - 1] = idMath::Sqrt( sum );
   2877 
   2878 	return true;
   2879 }
   2880 
   2881 /*
   2882 ============
   2883 idMatX::Cholesky_UpdateDecrement
   2884 
   2885   Updates the in-place Cholesky factorization to obtain the factors for the matrix with row r and column r removed.
   2886   v should store the row of the original matrix.
   2887 ============
   2888 */
   2889 bool idMatX::Cholesky_UpdateDecrement( const idVecX &v, int r ) {
   2890 	idVecX v1;
   2891 
   2892 	assert( numRows == numColumns );
   2893 	assert( v.GetSize() >= numRows );
   2894 	assert( r >= 0 && r < numRows );
   2895 
   2896 	v1.SetData( numRows, VECX_ALLOCA( numRows ) );
   2897 
   2898 	// update the row and column to identity
   2899 	v1 = -v;
   2900 	v1[r] += 1.0f;
   2901 
   2902 	// NOTE:	msvc compiler bug: the this pointer stored in edi is expected to stay
   2903 	//			untouched when calling Cholesky_UpdateRowColumn in the if statement
   2904 #if 0
   2905 	if ( !Cholesky_UpdateRowColumn( v1, r ) ) {
   2906 #else
   2907 	bool ret = Cholesky_UpdateRowColumn( v1, r );
   2908 	if ( !ret ) {
   2909 #endif
   2910 		return false;
   2911 	}
   2912 
   2913 	// physically remove the row and column
   2914 	Update_Decrement( r );
   2915 
   2916 	return true;
   2917 }
   2918 
   2919 /*
   2920 ============
   2921 idMatX::Cholesky_Solve
   2922 
   2923   Solve Ax = b with A factored in-place as: LL'
   2924 ============
   2925 */
   2926 void idMatX::Cholesky_Solve( idVecX &x, const idVecX &b ) const {
   2927 	int i, j;
   2928 	double sum;
   2929 
   2930 	assert( numRows == numColumns );
   2931 	assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
   2932 
   2933 	// solve L
   2934 	for ( i = 0; i < numRows; i++ ) {
   2935 		sum = b[i];
   2936 		for ( j = 0; j < i; j++ ) {
   2937 			sum -= (*this)[i][j] * x[j];
   2938 		}
   2939 		x[i] = sum / (*this)[i][i];
   2940 	}
   2941 
   2942 	// solve Lt
   2943 	for ( i = numRows - 1; i >= 0; i-- ) {
   2944 		sum = x[i];
   2945 		for ( j = i + 1; j < numRows; j++ ) {
   2946 			sum -= (*this)[j][i] * x[j];
   2947 		}
   2948 		x[i] = sum / (*this)[i][i];
   2949 	}
   2950 }
   2951 
   2952 /*
   2953 ============
   2954 idMatX::Cholesky_Inverse
   2955 
   2956   Calculates the inverse of the matrix which is factored in-place as: LL'
   2957 ============
   2958 */
   2959 void idMatX::Cholesky_Inverse( idMatX &inv ) const {
   2960 	int i, j;
   2961 	idVecX x, b;
   2962 
   2963 	assert( numRows == numColumns );
   2964 
   2965 	x.SetData( numRows, VECX_ALLOCA( numRows ) );
   2966 	b.SetData( numRows, VECX_ALLOCA( numRows ) );
   2967 	b.Zero();
   2968 	inv.SetSize( numRows, numColumns );
   2969 
   2970 	for ( i = 0; i < numRows; i++ ) {
   2971 
   2972 		b[i] = 1.0f;
   2973 		Cholesky_Solve( x, b );
   2974 		for ( j = 0; j < numRows; j++ ) {
   2975 			inv[j][i] = x[j];
   2976 		}
   2977 		b[i] = 0.0f;
   2978 	}
   2979 }
   2980 
   2981 /*
   2982 ============
   2983 idMatX::Cholesky_MultiplyFactors
   2984 
   2985   Multiplies the factors of the in-place Cholesky factorization to form the original matrix.
   2986 ============
   2987 */
   2988 void idMatX::Cholesky_MultiplyFactors( idMatX &m ) const {
   2989 	int r, i, j;
   2990 	double sum;
   2991 
   2992 	m.SetSize( numRows, numColumns );
   2993 
   2994 	for ( r = 0; r < numRows; r++ ) {
   2995 
   2996 		// calculate row of matrix
   2997 		for ( i = 0; i < numRows; i++ ) {
   2998 			sum = 0.0f;
   2999 			for ( j = 0; j <= i && j <= r; j++ ) {
   3000 				sum += (*this)[r][j] * (*this)[i][j];
   3001 			}
   3002 			m[r][i] = sum;
   3003 		}
   3004 	}
   3005 }
   3006 
   3007 /*
   3008 ============
   3009 idMatX::LDLT_Factor
   3010 
   3011   in-place factorization: LDL'
   3012   L is a triangular matrix stored in the lower triangle.
   3013   L has ones on the diagonal that are not stored.
   3014   D is a diagonal matrix stored on the diagonal.
   3015   The upper triangle is not cleared.
   3016   The initial matrix has to be symmetric.
   3017 ============
   3018 */
   3019 bool idMatX::LDLT_Factor() {
   3020 	int i, j, k;
   3021 	float *v;
   3022 	double d, sum;
   3023 
   3024 	assert( numRows == numColumns );
   3025 
   3026 	v = (float *) _alloca16( numRows * sizeof( float ) );
   3027 
   3028 	for ( i = 0; i < numRows; i++ ) {
   3029 
   3030 		sum = (*this)[i][i];
   3031 		for ( j = 0; j < i; j++ ) {
   3032 			d = (*this)[i][j];
   3033 		    v[j] = (*this)[j][j] * d;
   3034 		    sum -= v[j] * d;
   3035 		}
   3036 
   3037 		if ( sum == 0.0f ) {
   3038 			return false;
   3039 		}
   3040 
   3041 		(*this)[i][i] = sum;
   3042 		d = 1.0f / sum;
   3043 
   3044 		for ( j = i + 1; j < numRows; j++ ) {
   3045 		    sum = (*this)[j][i];
   3046 			for ( k = 0; k < i; k++ ) {
   3047 				sum -= (*this)[j][k] * v[k];
   3048 			}
   3049 		    (*this)[j][i] = sum * d;
   3050 		}
   3051 	}
   3052 
   3053 	return true;
   3054 }
   3055 
   3056 /*
   3057 ============
   3058 idMatX::LDLT_UpdateRankOne
   3059 
   3060   Updates the in-place LDL' factorization to obtain the factors for the matrix: LDL' + alpha * v * v'
   3061   If offset > 0 only the lower right corner starting at (offset, offset) is updated.
   3062 ============
   3063 */
   3064 bool idMatX::LDLT_UpdateRankOne( const idVecX &v, float alpha, int offset ) {
   3065 	int i, j;
   3066 	float *y;
   3067 	double diag, newDiag, beta, p, d;
   3068 
   3069 	assert( numRows == numColumns );
   3070 	assert( v.GetSize() >= numRows );
   3071 	assert( offset >= 0 && offset < numRows );
   3072 
   3073 	y = (float *) _alloca16( v.GetSize() * sizeof( float ) );
   3074 	memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
   3075 
   3076 	for ( i = offset; i < numColumns; i++ ) {
   3077 		p = y[i];
   3078 		diag = (*this)[i][i];
   3079 		(*this)[i][i] = newDiag = diag + alpha * p * p;
   3080 
   3081 		if ( newDiag == 0.0f ) {
   3082 			return false;
   3083 		}
   3084 
   3085 		alpha /= newDiag;
   3086 		beta = p * alpha;
   3087 		alpha *= diag;
   3088 
   3089 		for ( j = i+1; j < numRows; j++ ) {
   3090 
   3091 			d = (*this)[j][i];
   3092 
   3093 			y[j] -= p * d;
   3094 			d += beta * y[j];
   3095 
   3096 			(*this)[j][i] = d;
   3097 		}
   3098 	}
   3099 
   3100 	return true;
   3101 }
   3102 
   3103 /*
   3104 ============
   3105 idMatX::LDLT_UpdateRowColumn
   3106 
   3107   Updates the in-place LDL' factorization to obtain the factors for the matrix:
   3108 
   3109          [ 0  a  0 ]
   3110   LDL' + [ a  b  c ]
   3111          [ 0  c  0 ]
   3112 
   3113   where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
   3114 ============
   3115 */
   3116 bool idMatX::LDLT_UpdateRowColumn( const idVecX &v, int r ) {
   3117 	int i, j;
   3118 	double sum;
   3119 	float *original, *y;
   3120 	idVecX addSub;
   3121 
   3122 	assert( numRows == numColumns );
   3123 	assert( v.GetSize() >= numRows );
   3124 	assert( r >= 0 && r < numRows );
   3125 
   3126 	addSub.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
   3127 
   3128 	if ( r == 0 ) {
   3129 
   3130 		if ( numColumns == 1 ) {
   3131 			(*this)[0][0] += v[0];
   3132 			return true;
   3133 		}
   3134 		for ( i = 0; i < numColumns; i++ ) {
   3135 			addSub[i] = v[i];
   3136 		}
   3137 
   3138 	} else {
   3139 
   3140 		original = (float *) _alloca16( numColumns * sizeof( float ) );
   3141 		y = (float *) _alloca16( numColumns * sizeof( float ) );
   3142 
   3143 		// calculate original row/column of matrix
   3144 		for ( i = 0; i < r; i++ ) {
   3145 			y[i] = (*this)[r][i] * (*this)[i][i];
   3146 		}
   3147 		for ( i = 0; i < numColumns; i++ ) {
   3148 			if ( i < r ) {
   3149 				sum = (*this)[i][i] * (*this)[r][i];
   3150 			} else if ( i == r ) {
   3151 				sum = (*this)[r][r];
   3152 			} else {
   3153 				sum = (*this)[r][r] * (*this)[i][r];
   3154 			}
   3155 			for ( j = 0; j < i && j < r; j++ ) {
   3156 				sum += (*this)[i][j] * y[j];
   3157 			}
   3158 			original[i] = sum;
   3159 		}
   3160 
   3161 		// solve for y in L * y = original + v
   3162 		for ( i = 0; i < r; i++ ) {
   3163 			sum = original[i] + v[i];
   3164 			for ( j = 0; j < i; j++ ) {
   3165 				sum -= (*this)[i][j] * y[j];
   3166 			}
   3167 			y[i] = sum;
   3168 		}
   3169 
   3170 		// calculate new row of L
   3171 		for ( i = 0; i < r; i++ ) {
   3172 			(*this)[r][i] = y[i] / (*this)[i][i];
   3173 		}
   3174 
   3175 		// if the last row/column of the matrix is updated
   3176 		if ( r == numColumns - 1 ) {
   3177 			// only calculate new diagonal
   3178 			sum = original[r] + v[r];
   3179 			for ( j = 0; j < r; j++ ) {
   3180 				sum -= (*this)[r][j] * y[j];
   3181 			}
   3182 			if ( sum == 0.0f ) {
   3183 				return false;
   3184 			}
   3185 			(*this)[r][r] = sum;
   3186 			return true;
   3187 		}
   3188 
   3189 		// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
   3190 		for ( i = 0; i < r; i++ ) {
   3191 			y[i] = (*this)[r][i] * (*this)[i][i];
   3192 		}
   3193 		for ( i = r; i < numColumns; i++ ) {
   3194 			if ( i == r ) {
   3195 				sum = (*this)[r][r];
   3196 			} else {
   3197 				sum = (*this)[r][r] * (*this)[i][r];
   3198 			}
   3199 			for ( j = 0; j < r; j++ ) {
   3200 				sum += (*this)[i][j] * y[j];
   3201 			}
   3202 			addSub[i] = v[i] - ( sum - original[i] );
   3203 		}
   3204 	}
   3205 
   3206 	// add row/column to the lower right sub matrix starting at (r, r)
   3207 
   3208 #if 0
   3209 
   3210 	idVecX v1, v2;
   3211 	double d;
   3212 
   3213 	v1.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
   3214 	v2.SetData( numColumns, (float *) _alloca16( numColumns * sizeof( float ) ) );
   3215 
   3216 	d = idMath::SQRT_1OVER2;
   3217 	v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
   3218 	v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
   3219 	for ( i = r+1; i < numColumns; i++ ) {
   3220 		v1[i] = v2[i] = addSub[i] * d;
   3221 	}
   3222 
   3223 	// update
   3224 	if ( !LDLT_UpdateRankOne( v1, 1.0f, r ) ) {
   3225 		return false;
   3226 	}
   3227 	// downdate
   3228 	if ( !LDLT_UpdateRankOne( v2, -1.0f, r ) ) {
   3229 		return false;
   3230 	}
   3231 
   3232 #else
   3233 
   3234 	float *v1, *v2;
   3235 	double d, diag, newDiag, p1, p2, alpha1, alpha2, beta1, beta2;
   3236 
   3237 	v1 = (float *) _alloca16( numColumns * sizeof( float ) );
   3238 	v2 = (float *) _alloca16( numColumns * sizeof( float ) );
   3239 
   3240 	d = idMath::SQRT_1OVER2;
   3241 	v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
   3242 	v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
   3243 	for ( i = r+1; i < numColumns; i++ ) {
   3244 		v1[i] = v2[i] = addSub[i] * d;
   3245 	}
   3246 
   3247 	alpha1 = 1.0f;
   3248 	alpha2 = -1.0f;
   3249 
   3250 	// simultaneous update/downdate of the sub matrix starting at (r, r)
   3251 	for ( i = r; i < numColumns; i++ ) {
   3252 
   3253 		diag = (*this)[i][i];
   3254 		p1 = v1[i];
   3255 		newDiag = diag + alpha1 * p1 * p1;
   3256 
   3257 		if ( newDiag == 0.0f ) {
   3258 			return false;
   3259 		}
   3260 
   3261 		alpha1 /= newDiag;
   3262 		beta1 = p1 * alpha1;
   3263 		alpha1 *= diag;
   3264 
   3265 		diag = newDiag;
   3266 		p2 = v2[i];
   3267 		newDiag = diag + alpha2 * p2 * p2;
   3268 
   3269 		if ( newDiag == 0.0f ) {
   3270 			return false;
   3271 		}
   3272 
   3273 		alpha2 /= newDiag;
   3274 		beta2 = p2 * alpha2;
   3275 		alpha2 *= diag;
   3276 
   3277 		(*this)[i][i] = newDiag;
   3278 
   3279 		for ( j = i+1; j < numRows; j++ ) {
   3280 
   3281 			d = (*this)[j][i];
   3282 
   3283 			v1[j] -= p1 * d;
   3284 			d += beta1 * v1[j];
   3285 
   3286 			v2[j] -= p2 * d;
   3287 			d += beta2 * v2[j];
   3288 
   3289 			(*this)[j][i] = d;
   3290 		}
   3291 	}
   3292 
   3293 #endif
   3294 
   3295 	return true;
   3296 }
   3297 
   3298 /*
   3299 ============
   3300 idMatX::LDLT_UpdateIncrement
   3301 
   3302   Updates the in-place LDL' factorization to obtain the factors for the matrix:
   3303 
   3304   [ A  a ]
   3305   [ a  b ]
   3306 
   3307   where: a = v[0,numRows-1], b = v[numRows]
   3308 ============
   3309 */
   3310 bool idMatX::LDLT_UpdateIncrement( const idVecX &v ) {
   3311 	int i, j;
   3312 	float *x;
   3313 	double sum, d;
   3314 
   3315 	assert( numRows == numColumns );
   3316 	assert( v.GetSize() >= numRows+1 );
   3317 
   3318 	ChangeSize( numRows+1, numColumns+1, false );
   3319 
   3320 	x = (float *) _alloca16( numRows * sizeof( float ) );
   3321 
   3322 	// solve for x in L * x = v
   3323 	for ( i = 0; i < numRows - 1; i++ ) {
   3324 		sum = v[i];
   3325 		for ( j = 0; j < i; j++ ) {
   3326 			sum -= (*this)[i][j] * x[j];
   3327 		}
   3328 		x[i] = sum;
   3329 	}
   3330 
   3331 	// calculate new row of L and calculate the diagonal entry
   3332 	sum = v[numRows - 1];
   3333 	for ( i = 0; i < numRows - 1; i++ ) {
   3334 		(*this)[numRows - 1][i] = d = x[i] / (*this)[i][i];
   3335 		sum -= d * x[i];
   3336 	}
   3337 
   3338 	if ( sum == 0.0f ) {
   3339 		return false;
   3340 	}
   3341 
   3342 	// store the diagonal entry
   3343 	(*this)[numRows - 1][numRows - 1] = sum;
   3344 
   3345 	return true;
   3346 }
   3347 
   3348 /*
   3349 ============
   3350 idMatX::LDLT_UpdateDecrement
   3351 
   3352   Updates the in-place LDL' factorization to obtain the factors for the matrix with row r and column r removed.
   3353   v should store the row of the original matrix.
   3354 ============
   3355 */
   3356 bool idMatX::LDLT_UpdateDecrement( const idVecX &v, int r ) {
   3357 	idVecX v1;
   3358 
   3359 	assert( numRows == numColumns );
   3360 	assert( v.GetSize() >= numRows );
   3361 	assert( r >= 0 && r < numRows );
   3362 
   3363 	v1.SetData( numRows, VECX_ALLOCA( numRows ) );
   3364 
   3365 	// update the row and column to identity
   3366 	v1 = -v;
   3367 	v1[r] += 1.0f;
   3368 
   3369 	// NOTE:	msvc compiler bug: the this pointer stored in edi is expected to stay
   3370 	//			untouched when calling LDLT_UpdateRowColumn in the if statement
   3371 #if 0
   3372 	if ( !LDLT_UpdateRowColumn( v1, r ) ) {
   3373 #else
   3374 	bool ret = LDLT_UpdateRowColumn( v1, r );
   3375 	if ( !ret ) {
   3376 #endif
   3377 		return false;
   3378 	}
   3379 
   3380 	// physically remove the row and column
   3381 	Update_Decrement( r );
   3382 
   3383 	return true;
   3384 }
   3385 
   3386 /*
   3387 ============
   3388 idMatX::LDLT_Solve
   3389 
   3390   Solve Ax = b with A factored in-place as: LDL'
   3391 ============
   3392 */
   3393 void idMatX::LDLT_Solve( idVecX &x, const idVecX &b ) const {
   3394 	int i, j;
   3395 	double sum;
   3396 
   3397 	assert( numRows == numColumns );
   3398 	assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
   3399 
   3400 	// solve L
   3401 	for ( i = 0; i < numRows; i++ ) {
   3402 		sum = b[i];
   3403 		for ( j = 0; j < i; j++ ) {
   3404 			sum -= (*this)[i][j] * x[j];
   3405 		}
   3406 		x[i] = sum;
   3407 	}
   3408 
   3409 	// solve D
   3410 	for ( i = 0; i < numRows; i++ ) {
   3411 		x[i] /= (*this)[i][i];
   3412 	}
   3413 
   3414 	// solve Lt
   3415 	for ( i = numRows - 2; i >= 0; i-- ) {
   3416 		sum = x[i];
   3417 		for ( j = i + 1; j < numRows; j++ ) {
   3418 			sum -= (*this)[j][i] * x[j];
   3419 		}
   3420 		x[i] = sum;
   3421 	}
   3422 }
   3423 
   3424 /*
   3425 ============
   3426 idMatX::LDLT_Inverse
   3427 
   3428   Calculates the inverse of the matrix which is factored in-place as: LDL'
   3429 ============
   3430 */
   3431 void idMatX::LDLT_Inverse( idMatX &inv ) const {
   3432 	int i, j;
   3433 	idVecX x, b;
   3434 
   3435 	assert( numRows == numColumns );
   3436 
   3437 	x.SetData( numRows, VECX_ALLOCA( numRows ) );
   3438 	b.SetData( numRows, VECX_ALLOCA( numRows ) );
   3439 	b.Zero();
   3440 	inv.SetSize( numRows, numColumns );
   3441 
   3442 	for ( i = 0; i < numRows; i++ ) {
   3443 
   3444 		b[i] = 1.0f;
   3445 		LDLT_Solve( x, b );
   3446 		for ( j = 0; j < numRows; j++ ) {
   3447 			inv[j][i] = x[j];
   3448 		}
   3449 		b[i] = 0.0f;
   3450 	}
   3451 }
   3452 
   3453 /*
   3454 ============
   3455 idMatX::LDLT_UnpackFactors
   3456 
   3457   Unpacks the in-place LDL' factorization.
   3458 ============
   3459 */
   3460 void idMatX::LDLT_UnpackFactors( idMatX &L, idMatX &D ) const {
   3461 	int i, j;
   3462 
   3463 	L.Zero( numRows, numColumns );
   3464 	D.Zero( numRows, numColumns );
   3465 	for ( i = 0; i < numRows; i++ ) {
   3466 		for ( j = 0; j < i; j++ ) {
   3467 			L[i][j] = (*this)[i][j];
   3468 		}
   3469 		L[i][i] = 1.0f;
   3470 		D[i][i] = (*this)[i][i];
   3471 	}
   3472 }
   3473 
   3474 /*
   3475 ============
   3476 idMatX::LDLT_MultiplyFactors
   3477 
   3478   Multiplies the factors of the in-place LDL' factorization to form the original matrix.
   3479 ============
   3480 */
   3481 void idMatX::LDLT_MultiplyFactors( idMatX &m ) const {
   3482 	int r, i, j;
   3483 	float *v;
   3484 	double sum;
   3485 
   3486 	v = (float *) _alloca16( numRows * sizeof( float ) );
   3487 	m.SetSize( numRows, numColumns );
   3488 
   3489 	for ( r = 0; r < numRows; r++ ) {
   3490 
   3491 		// calculate row of matrix
   3492 		for ( i = 0; i < r; i++ ) {
   3493 			v[i] = (*this)[r][i] * (*this)[i][i];
   3494 		}
   3495 		for ( i = 0; i < numColumns; i++ ) {
   3496 			if ( i < r ) {
   3497 				sum = (*this)[i][i] * (*this)[r][i];
   3498 			} else if ( i == r ) {
   3499 				sum = (*this)[r][r];
   3500 			} else {
   3501 				sum = (*this)[r][r] * (*this)[i][r];
   3502 			}
   3503 			for ( j = 0; j < i && j < r; j++ ) {
   3504 				sum += (*this)[i][j] * v[j];
   3505 			}
   3506 			m[r][i] = sum;
   3507 		}
   3508 	}
   3509 }
   3510 
   3511 /*
   3512 ============
   3513 idMatX::TriDiagonal_ClearTriangles
   3514 ============
   3515 */
   3516 void idMatX::TriDiagonal_ClearTriangles() {
   3517 	int i, j;
   3518 
   3519 	assert( numRows == numColumns );
   3520 	for ( i = 0; i < numRows-2; i++ ) {
   3521 		for ( j = i+2; j < numColumns; j++ ) {
   3522 			(*this)[i][j] = 0.0f;
   3523 			(*this)[j][i] = 0.0f;
   3524 		}
   3525 	}
   3526 }
   3527 
   3528 /*
   3529 ============
   3530 idMatX::TriDiagonal_Solve
   3531 
   3532   Solve Ax = b with A being tridiagonal.
   3533 ============
   3534 */
   3535 bool idMatX::TriDiagonal_Solve( idVecX &x, const idVecX &b ) const {
   3536 	int i;
   3537 	float d;
   3538 	idVecX tmp;
   3539 
   3540 	assert( numRows == numColumns );
   3541 	assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
   3542 
   3543 	tmp.SetData( numRows, VECX_ALLOCA( numRows ) );
   3544 
   3545 	d = (*this)[0][0];
   3546 	if ( d == 0.0f ) {
   3547 		return false;
   3548 	}
   3549 	d = 1.0f / d;
   3550 	x[0] = b[0] * d;
   3551 	for ( i = 1; i < numRows; i++ ) {
   3552 		tmp[i] = (*this)[i-1][i] * d;
   3553 		d = (*this)[i][i] - (*this)[i][i-1] * tmp[i];
   3554 		if ( d == 0.0f ) {
   3555 			return false;
   3556 		}
   3557 		d = 1.0f / d;
   3558 		x[i] = ( b[i] - (*this)[i][i-1] * x[i-1] ) * d;
   3559 	}
   3560 	for ( i = numRows - 2; i >= 0; i-- ) {
   3561 		x[i] -= tmp[i+1] * x[i+1];
   3562 	}
   3563 	return true;
   3564 }
   3565 
   3566 /*
   3567 ============
   3568 idMatX::TriDiagonal_Inverse
   3569 
   3570   Calculates the inverse of a tri-diagonal matrix.
   3571 ============
   3572 */
   3573 void idMatX::TriDiagonal_Inverse( idMatX &inv ) const {
   3574 	int i, j;
   3575 	idVecX x, b;
   3576 
   3577 	assert( numRows == numColumns );
   3578 
   3579 	x.SetData( numRows, VECX_ALLOCA( numRows ) );
   3580 	b.SetData( numRows, VECX_ALLOCA( numRows ) );
   3581 	b.Zero();
   3582 	inv.SetSize( numRows, numColumns );
   3583 
   3584 	for ( i = 0; i < numRows; i++ ) {
   3585 
   3586 		b[i] = 1.0f;
   3587 		TriDiagonal_Solve( x, b );
   3588 		for ( j = 0; j < numRows; j++ ) {
   3589 			inv[j][i] = x[j];
   3590 		}
   3591 		b[i] = 0.0f;
   3592 	}
   3593 }
   3594 
   3595 /*
   3596 ============
   3597 idMatX::HouseholderReduction
   3598 
   3599   Householder reduction to symmetric tri-diagonal form.
   3600   The original matrix is replaced by an orthogonal matrix effecting the accumulated householder transformations.
   3601   The diagonal elements of the diagonal matrix are stored in diag.
   3602   The off-diagonal elements of the diagonal matrix are stored in subd.
   3603   The initial matrix has to be symmetric.
   3604 ============
   3605 */
   3606 void idMatX::HouseholderReduction( idVecX &diag, idVecX &subd ) {
   3607 	int i0, i1, i2, i3;
   3608 	float h, f, g, invH, halfFdivH, scale, invScale, sum;
   3609 
   3610 	assert( numRows == numColumns );
   3611 
   3612 	diag.SetSize( numRows );
   3613 	subd.SetSize( numRows );
   3614 
   3615 	for ( i0 = numRows-1, i3 = numRows-2; i0 >= 1; i0--, i3-- ) {
   3616 		h = 0.0f;
   3617 		scale = 0.0f;
   3618 
   3619 		if ( i3 > 0 ) {
   3620 			for ( i2 = 0; i2 <= i3; i2++ ) {
   3621 				scale += idMath::Fabs( (*this)[i0][i2] );
   3622 			}
   3623 			if ( scale == 0 ) {
   3624 				subd[i0] = (*this)[i0][i3];
   3625 			} else {
   3626 				invScale = 1.0f / scale;
   3627 				for (i2 = 0; i2 <= i3; i2++)
   3628 				{
   3629 					(*this)[i0][i2] *= invScale;
   3630 					h += (*this)[i0][i2] * (*this)[i0][i2];
   3631 				}
   3632 				f = (*this)[i0][i3];
   3633 				g = idMath::Sqrt( h );
   3634 				if ( f > 0.0f ) {
   3635 					g = -g;
   3636 				}
   3637 				subd[i0] = scale * g;
   3638 				h -= f * g;
   3639 				(*this)[i0][i3] = f - g;
   3640 				f = 0.0f;
   3641 				invH = 1.0f / h;
   3642 				for (i1 = 0; i1 <= i3; i1++) {
   3643 					(*this)[i1][i0] = (*this)[i0][i1] * invH;
   3644 					g = 0.0f;
   3645 					for (i2 = 0; i2 <= i1; i2++) {
   3646 						g += (*this)[i1][i2] * (*this)[i0][i2];
   3647 					}
   3648 					for (i2 = i1+1; i2 <= i3; i2++) {
   3649 						g += (*this)[i2][i1] * (*this)[i0][i2];
   3650 					}
   3651 					subd[i1] = g * invH;
   3652 					f += subd[i1] * (*this)[i0][i1];
   3653 				}
   3654 				halfFdivH = 0.5f * f * invH;
   3655 				for ( i1 = 0; i1 <= i3; i1++ ) {
   3656 					f = (*this)[i0][i1];
   3657 					g = subd[i1] - halfFdivH * f;
   3658 					subd[i1] = g;
   3659 					for ( i2 = 0; i2 <= i1; i2++ ) {
   3660 						(*this)[i1][i2] -= f * subd[i2] + g * (*this)[i0][i2];
   3661 					}
   3662 				}
   3663             }
   3664 		} else {
   3665 			subd[i0] = (*this)[i0][i3];
   3666 		}
   3667 
   3668 		diag[i0] = h;
   3669 	}
   3670 
   3671 	diag[0] = 0.0f;
   3672 	subd[0] = 0.0f;
   3673 	for ( i0 = 0, i3 = -1; i0 <= numRows-1; i0++, i3++ ) {
   3674 		if ( diag[i0] ) {
   3675 			for ( i1 = 0; i1 <= i3; i1++ ) {
   3676 				sum = 0.0f;
   3677 				for (i2 = 0; i2 <= i3; i2++) {
   3678 					sum += (*this)[i0][i2] * (*this)[i2][i1];
   3679 				}
   3680 				for ( i2 = 0; i2 <= i3; i2++ ) {
   3681 					(*this)[i2][i1] -= sum * (*this)[i2][i0];
   3682 				}
   3683 			}
   3684 		}
   3685 		diag[i0] = (*this)[i0][i0];
   3686 		(*this)[i0][i0] = 1.0f;
   3687 		for ( i1 = 0; i1 <= i3; i1++ ) {
   3688 			(*this)[i1][i0] = 0.0f;
   3689 			(*this)[i0][i1] = 0.0f;
   3690 		}
   3691 	}
   3692 
   3693 	// re-order
   3694 	for ( i0 = 1, i3 = 0; i0 < numRows; i0++, i3++ ) {
   3695 		subd[i3] = subd[i0];
   3696 	}
   3697 	subd[numRows-1] = 0.0f;
   3698 }
   3699 
   3700 /*
   3701 ============
   3702 idMatX::QL
   3703 
   3704   QL algorithm with implicit shifts to determine the eigenvalues and eigenvectors of a symmetric tri-diagonal matrix.
   3705   diag contains the diagonal elements of the symmetric tri-diagonal matrix on input and is overwritten with the eigenvalues.
   3706   subd contains the off-diagonal elements of the symmetric tri-diagonal matrix and is destroyed.
   3707   This matrix has to be either the identity matrix to determine the eigenvectors for a symmetric tri-diagonal matrix,
   3708   or the matrix returned by the Householder reduction to determine the eigenvalues for the original symmetric matrix.
   3709 ============
   3710 */
   3711 bool idMatX::QL( idVecX &diag, idVecX &subd ) {
   3712     const int maxIter = 32;
   3713 	int i0, i1, i2, i3;
   3714 	float a, b, f, g, r, p, s, c;
   3715 
   3716 	assert( numRows == numColumns );
   3717 
   3718 	for ( i0 = 0; i0 < numRows; i0++ ) {
   3719 		for ( i1 = 0; i1 < maxIter; i1++ ) {
   3720 			for ( i2 = i0; i2 <= numRows - 2; i2++ ) {
   3721 				a = idMath::Fabs( diag[i2] ) + idMath::Fabs( diag[i2+1] );
   3722 				if ( idMath::Fabs( subd[i2] ) + a == a ) {
   3723 					break;
   3724 				}
   3725 			}
   3726 			if ( i2 == i0 ) {
   3727 				break;
   3728 			}
   3729 
   3730 			g = ( diag[i0+1] - diag[i0] ) / ( 2.0f * subd[i0] );
   3731 			r = idMath::Sqrt( g * g + 1.0f );
   3732 			if ( g < 0.0f ) {
   3733 				g = diag[i2] - diag[i0] + subd[i0] / ( g - r );
   3734 			} else {
   3735 				g = diag[i2] - diag[i0] + subd[i0] / ( g + r );
   3736 			}
   3737 			s = 1.0f;
   3738 			c = 1.0f;
   3739 			p = 0.0f;
   3740 			for ( i3 = i2 - 1; i3 >= i0; i3-- ) {
   3741 				f = s * subd[i3];
   3742 				b = c * subd[i3];
   3743 				if ( idMath::Fabs( f ) >= idMath::Fabs( g ) ) {
   3744 					c = g / f;
   3745 					r = idMath::Sqrt( c * c + 1.0f );
   3746 					subd[i3+1] = f * r;
   3747 					s = 1.0f / r;
   3748 					c *= s;
   3749 				} else {
   3750 					s = f / g;
   3751 					r = idMath::Sqrt( s * s + 1.0f );
   3752 					subd[i3+1] = g * r;
   3753 					c = 1.0f / r;
   3754 					s *= c;
   3755 				}
   3756 				g = diag[i3+1] - p;
   3757 				r = ( diag[i3] - g ) * s + 2.0f * b * c;
   3758 				p = s * r;
   3759 				diag[i3+1] = g + p;
   3760 				g = c * r - b;
   3761 
   3762 				for ( int i4 = 0; i4 < numRows; i4++ ) {
   3763 					f = (*this)[i4][i3+1];
   3764 					(*this)[i4][i3+1] = s * (*this)[i4][i3] + c * f;
   3765 					(*this)[i4][i3] = c * (*this)[i4][i3] - s * f;
   3766 				}
   3767 			}
   3768 			diag[i0] -= p;
   3769 			subd[i0] = g;
   3770 			subd[i2] = 0.0f;
   3771 		}
   3772 		if ( i1 == maxIter ) {
   3773 			return false;
   3774 		}
   3775 	}
   3776 	return true;
   3777 }
   3778 
   3779 /*
   3780 ============
   3781 idMatX::Eigen_SolveSymmetricTriDiagonal
   3782 
   3783   Determine eigen values and eigen vectors for a symmetric tri-diagonal matrix.
   3784   The eigen values are stored in 'eigenValues'.
   3785   Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
   3786   The initial matrix has to be symmetric tri-diagonal.
   3787 ============
   3788 */
   3789 bool idMatX::Eigen_SolveSymmetricTriDiagonal( idVecX &eigenValues ) {
   3790 	int i;
   3791 	idVecX subd;
   3792 
   3793 	assert( numRows == numColumns );
   3794 
   3795 	subd.SetData( numRows, VECX_ALLOCA( numRows ) );
   3796 	eigenValues.SetSize( numRows );
   3797 
   3798 	for ( i = 0; i < numRows-1; i++ ) {
   3799 		eigenValues[i] = (*this)[i][i];
   3800 		subd[i] = (*this)[i+1][i];
   3801 	}
   3802 	eigenValues[numRows-1] = (*this)[numRows-1][numRows-1];
   3803 
   3804 	Identity();
   3805 
   3806 	return QL( eigenValues, subd );
   3807 }
   3808 
   3809 /*
   3810 ============
   3811 idMatX::Eigen_SolveSymmetric
   3812 
   3813   Determine eigen values and eigen vectors for a symmetric matrix.
   3814   The eigen values are stored in 'eigenValues'.
   3815   Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
   3816   The initial matrix has to be symmetric.
   3817 ============
   3818 */
   3819 bool idMatX::Eigen_SolveSymmetric( idVecX &eigenValues ) {
   3820 	idVecX subd;
   3821 
   3822 	assert( numRows == numColumns );
   3823 
   3824 	subd.SetData( numRows, VECX_ALLOCA( numRows ) );
   3825 	eigenValues.SetSize( numRows );
   3826 
   3827 	HouseholderReduction( eigenValues, subd );
   3828 	return QL( eigenValues, subd );
   3829 }
   3830 
   3831 /*
   3832 ============
   3833 idMatX::HessenbergReduction
   3834 
   3835   Reduction to Hessenberg form.
   3836 ============
   3837 */
   3838 void idMatX::HessenbergReduction( idMatX &H ) {
   3839 	int i, j, m;
   3840 	int low = 0;
   3841 	int high = numRows - 1;
   3842 	float scale, f, g, h;
   3843 	idVecX v;
   3844 
   3845 	v.SetData( numRows, VECX_ALLOCA( numRows ) );
   3846 
   3847 	for ( m = low + 1; m <= high - 1; m++ ) {
   3848 
   3849 		scale = 0.0f;
   3850 		for ( i = m; i <= high; i++ ) {
   3851 			scale = scale + idMath::Fabs( H[i][m-1] );
   3852 		}
   3853 		if ( scale != 0.0f ) {
   3854 
   3855 			// compute Householder transformation.
   3856 			h = 0.0f;
   3857 			for ( i = high; i >= m; i-- ) {
   3858 				v[i] = H[i][m-1] / scale;
   3859 				h += v[i] * v[i];
   3860 			}
   3861 			g = idMath::Sqrt( h );
   3862 			if ( v[m] > 0.0f ) {
   3863 				g = -g;
   3864 			}
   3865 			h = h - v[m] * g;
   3866 			v[m] = v[m] - g;
   3867 
   3868 			// apply Householder similarity transformation
   3869 			// H = (I-u*u'/h)*H*(I-u*u')/h)
   3870 			for ( j = m; j < numRows; j++) {
   3871 				f = 0.0f;
   3872 				for ( i = high; i >= m; i-- ) {
   3873 					f += v[i] * H[i][j];
   3874 				}
   3875 				f = f / h;
   3876 				for ( i = m; i <= high; i++ ) {
   3877 					H[i][j] -= f * v[i];
   3878 				}
   3879 			}
   3880 
   3881 			for ( i = 0; i <= high; i++ ) {
   3882 				f = 0.0f;
   3883 				for ( j = high; j >= m; j-- ) {
   3884 					f += v[j] * H[i][j];
   3885 				}
   3886 				f = f / h;
   3887 				for ( j = m; j <= high; j++ ) {
   3888 					H[i][j] -= f * v[j];
   3889 				}
   3890 			}
   3891 			v[m] = scale * v[m];
   3892 			H[m][m-1] = scale * g;
   3893 		}
   3894 	}
   3895 
   3896 	// accumulate transformations
   3897 	Identity();
   3898 	for ( int m = high - 1; m >= low + 1; m-- ) {
   3899 		if ( H[m][m-1] != 0.0f ) {
   3900 			for ( i = m + 1; i <= high; i++ ) {
   3901 				v[i] = H[i][m-1];
   3902 			}
   3903 			for ( j = m; j <= high; j++ ) {
   3904 				g = 0.0f;
   3905 				for ( i = m; i <= high; i++ ) {
   3906 					g += v[i] * (*this)[i][j];
   3907 				}
   3908 				// float division to avoid possible underflow
   3909 				g = ( g / v[m] ) / H[m][m-1];
   3910 				for ( i = m; i <= high; i++ ) {
   3911 					(*this)[i][j] += g * v[i];
   3912 				}
   3913 			}
   3914 		}
   3915 	}
   3916 }
   3917 
   3918 /*
   3919 ============
   3920 idMatX::ComplexDivision
   3921 
   3922   Complex scalar division.
   3923 ============
   3924 */
   3925 void idMatX::ComplexDivision( float xr, float xi, float yr, float yi, float &cdivr, float &cdivi ) {
   3926 	float r, d;
   3927 	if ( idMath::Fabs( yr ) > idMath::Fabs( yi ) ) {
   3928 		r = yi / yr;
   3929 		d = yr + r * yi;
   3930 		cdivr = ( xr + r * xi ) / d;
   3931 		cdivi = ( xi - r * xr ) / d;
   3932 	} else {
   3933 		r = yr / yi;
   3934 		d = yi + r * yr;
   3935 		cdivr = ( r * xr + xi ) / d;
   3936 		cdivi = ( r * xi - xr ) / d;
   3937 	}
   3938 }
   3939 
   3940 /*
   3941 ============
   3942 idMatX::HessenbergToRealSchur
   3943 
   3944   Reduction from Hessenberg to real Schur form.
   3945 ============
   3946 */
   3947 bool idMatX::HessenbergToRealSchur( idMatX &H, idVecX &realEigenValues, idVecX &imaginaryEigenValues ) {
   3948 	int i, j, k;
   3949 	int n = numRows - 1;
   3950 	int low = 0;
   3951 	int high = numRows - 1;
   3952 	float eps = 2e-16f, exshift = 0.0f;
   3953 	float p = 0.0f, q = 0.0f, r = 0.0f, s = 0.0f, z = 0.0f, t, w, x, y;
   3954 
   3955 	// store roots isolated by balanc and compute matrix norm
   3956 	float norm = 0.0f;
   3957 	for ( i = 0; i < numRows; i++ ) {
   3958 		if ( i < low || i > high ) {
   3959 			realEigenValues[i] = H[i][i];
   3960 			imaginaryEigenValues[i] = 0.0f;
   3961 		}
   3962 		for ( j = Max( i - 1, 0 ); j < numRows; j++ ) {
   3963 			norm = norm + idMath::Fabs( H[i][j] );
   3964 		}
   3965 	}
   3966 
   3967 	int iter = 0;
   3968 	while( n >= low ) {
   3969 
   3970 		// look for single small sub-diagonal element
   3971 		int l = n;
   3972 		while ( l > low ) {
   3973 			s = idMath::Fabs( H[l-1][l-1] ) + idMath::Fabs( H[l][l] );
   3974 			if ( s == 0.0f ) {
   3975 				s = norm;
   3976 			}
   3977 			if ( idMath::Fabs( H[l][l-1] ) < eps * s ) {
   3978 				break;
   3979 			}
   3980 			l--;
   3981 		}
   3982 	   
   3983 		// check for convergence
   3984 		if ( l == n ) {			// one root found
   3985 			H[n][n] = H[n][n] + exshift;
   3986 			realEigenValues[n] = H[n][n];
   3987 			imaginaryEigenValues[n] = 0.0f;
   3988 			n--;
   3989 			iter = 0;
   3990 		} else if ( l == n-1 ) {	// two roots found
   3991 			w = H[n][n-1] * H[n-1][n];
   3992 			p = ( H[n-1][n-1] - H[n][n] ) / 2.0f;
   3993 			q = p * p + w;
   3994 			z = idMath::Sqrt( idMath::Fabs( q ) );
   3995 			H[n][n] = H[n][n] + exshift;
   3996 			H[n-1][n-1] = H[n-1][n-1] + exshift;
   3997 			x = H[n][n];
   3998 
   3999 			if ( q >= 0.0f ) {		// real pair
   4000 				if ( p >= 0.0f ) {
   4001 					z = p + z;
   4002 				} else {
   4003 					z = p - z;
   4004 				}
   4005 				realEigenValues[n-1] = x + z;
   4006 				realEigenValues[n] = realEigenValues[n-1];
   4007 				if ( z != 0.0f ) {
   4008 					realEigenValues[n] = x - w / z;
   4009 				}
   4010 				imaginaryEigenValues[n-1] = 0.0f;
   4011 				imaginaryEigenValues[n] = 0.0f;
   4012 				x = H[n][n-1];
   4013 				s = idMath::Fabs( x ) + idMath::Fabs( z );
   4014 				p = x / s;
   4015 				q = z / s;
   4016 				r = idMath::Sqrt( p * p + q * q );
   4017 				p = p / r;
   4018 				q = q / r;
   4019 
   4020 				// modify row
   4021 				for ( j = n-1; j < numRows; j++ ) {
   4022 					z = H[n-1][j];
   4023 					H[n-1][j] = q * z + p * H[n][j];
   4024 					H[n][j] = q * H[n][j] - p * z;
   4025 				}
   4026 
   4027 				// modify column
   4028 				for ( i = 0; i <= n; i++ ) {
   4029 					z = H[i][n-1];
   4030 					H[i][n-1] = q * z + p * H[i][n];
   4031 					H[i][n] = q * H[i][n] - p * z;
   4032 				}
   4033 
   4034 				// accumulate transformations
   4035 				for ( i = low; i <= high; i++ ) {
   4036 					z = (*this)[i][n-1];
   4037 					(*this)[i][n-1] = q * z + p * (*this)[i][n];
   4038 					(*this)[i][n] = q * (*this)[i][n] - p * z;
   4039 				}
   4040 			} else {		// complex pair
   4041 				realEigenValues[n-1] = x + p;
   4042 				realEigenValues[n] = x + p;
   4043 				imaginaryEigenValues[n-1] = z;
   4044 				imaginaryEigenValues[n] = -z;
   4045 			}
   4046 			n = n - 2;
   4047 			iter = 0;
   4048 
   4049 		} else {	// no convergence yet
   4050 
   4051 			// form shift
   4052 			x = H[n][n];
   4053 			y = 0.0f;
   4054 			w = 0.0f;
   4055 			if ( l < n ) {
   4056 				y = H[n-1][n-1];
   4057 				w = H[n][n-1] * H[n-1][n];
   4058 			}
   4059 
   4060 			// Wilkinson's original ad hoc shift
   4061 			if ( iter == 10 ) {
   4062 				exshift += x;
   4063 				for ( i = low; i <= n; i++ ) {
   4064 					H[i][i] -= x;
   4065 				}
   4066 				s = idMath::Fabs( H[n][n-1] ) + idMath::Fabs( H[n-1][n-2] );
   4067 				x = y = 0.75f * s;
   4068 				w = -0.4375f * s * s;
   4069 			}
   4070 
   4071 			// new ad hoc shift
   4072 			if ( iter == 30 ) {
   4073 				s = ( y - x ) / 2.0f;
   4074 				s = s * s + w;
   4075 				if ( s > 0 ) {
   4076 					s = idMath::Sqrt( s );
   4077 					if ( y < x ) {
   4078 						s = -s;
   4079 					}
   4080 					s = x - w / ( ( y - x ) / 2.0f + s );
   4081 					for ( i = low; i <= n; i++ ) {
   4082 						H[i][i] -= s;
   4083 					}
   4084 					exshift += s;
   4085 					x = y = w = 0.964f;
   4086 				}
   4087 			}
   4088 
   4089 			iter = iter + 1;
   4090 
   4091 			// look for two consecutive small sub-diagonal elements
   4092 			int m;
   4093 			for( m = n-2; m >= l; m-- ) {
   4094 				z = H[m][m];
   4095 				r = x - z;
   4096 				s = y - z;
   4097 				p = ( r * s - w ) / H[m+1][m] + H[m][m+1];
   4098 				q = H[m+1][m+1] - z - r - s;
   4099 				r = H[m+2][m+1];
   4100 				s = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
   4101 				p = p / s;
   4102 				q = q / s;
   4103 				r = r / s;
   4104 				if ( m == l ) {
   4105 					break;
   4106 				}
   4107 				if ( idMath::Fabs( H[m][m-1] ) * ( idMath::Fabs( q ) + idMath::Fabs( r ) ) <
   4108 						eps * ( idMath::Fabs( p ) * ( idMath::Fabs( H[m-1][m-1] ) + idMath::Fabs( z ) + idMath::Fabs( H[m+1][m+1] ) ) ) ) {
   4109 					break;
   4110 				}
   4111 			}
   4112 
   4113 			for ( i = m+2; i <= n; i++ ) {
   4114 				H[i][i-2] = 0.0f;
   4115 				if ( i > m+2 ) {
   4116 					H[i][i-3] = 0.0f;
   4117 				}
   4118 			}
   4119 
   4120 			// double QR step involving rows l:n and columns m:n
   4121 			for ( k = m; k <= n-1; k++ ) {
   4122 				bool notlast = ( k != n-1 );
   4123 				if ( k != m ) {
   4124 					p = H[k][k-1];
   4125 					q = H[k+1][k-1];
   4126 					r = ( notlast ? H[k+2][k-1] : 0.0f );
   4127 					x = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
   4128 					if ( x != 0.0f ) {
   4129 						p = p / x;
   4130 						q = q / x;
   4131 						r = r / x;
   4132 					}
   4133 				}
   4134 				if ( x == 0.0f ) {
   4135 					break;
   4136 				}
   4137 				s = idMath::Sqrt( p * p + q * q + r * r );
   4138 				if ( p < 0.0f ) {
   4139 					s = -s;
   4140 				}
   4141 				if ( s != 0.0f ) {
   4142 					if ( k != m ) {
   4143 						H[k][k-1] = -s * x;
   4144 					} else if ( l != m ) {
   4145 						H[k][k-1] = -H[k][k-1];
   4146 					}
   4147 					p = p + s;
   4148 					x = p / s;
   4149 					y = q / s;
   4150 					z = r / s;
   4151 					q = q / p;
   4152 					r = r / p;
   4153 
   4154 					// modify row
   4155 					for ( j = k; j < numRows; j++ ) {
   4156 						p = H[k][j] + q * H[k+1][j];
   4157 						if ( notlast ) {
   4158 							p = p + r * H[k+2][j];
   4159 							H[k+2][j] = H[k+2][j] - p * z;
   4160 						}
   4161 						H[k][j] = H[k][j] - p * x;
   4162 						H[k+1][j] = H[k+1][j] - p * y;
   4163 					}
   4164 
   4165 					// modify column
   4166 					for ( i = 0; i <= Min( n, k + 3 ); i++ ) {
   4167 						p = x * H[i][k] + y * H[i][k+1];
   4168 						if ( notlast ) {
   4169 							p = p + z * H[i][k+2];
   4170 							H[i][k+2] = H[i][k+2] - p * r;
   4171 						}
   4172 						H[i][k] = H[i][k] - p;
   4173 						H[i][k+1] = H[i][k+1] - p * q;
   4174 					}
   4175 
   4176 					// accumulate transformations
   4177 					for ( i = low; i <= high; i++ ) {
   4178 						p = x * (*this)[i][k] + y * (*this)[i][k+1];
   4179 						if ( notlast ) {
   4180 							p = p + z * (*this)[i][k+2];
   4181 							(*this)[i][k+2] = (*this)[i][k+2] - p * r;
   4182 						}
   4183 						(*this)[i][k] = (*this)[i][k] - p;
   4184 						(*this)[i][k+1] = (*this)[i][k+1] - p * q;
   4185 					}
   4186 				}
   4187 			}
   4188 		}
   4189 	}
   4190 	
   4191 	// backsubstitute to find vectors of upper triangular form
   4192 	if ( norm == 0.0f ) {
   4193 		return false;
   4194 	}
   4195 
   4196 	for ( n = numRows-1; n >= 0; n-- ) {
   4197 		p = realEigenValues[n];
   4198 		q = imaginaryEigenValues[n];
   4199 
   4200 		if ( q == 0.0f ) {		// real vector
   4201 			int l = n;
   4202 			H[n][n] = 1.0f;
   4203 			for ( i = n-1; i >= 0; i-- ) {
   4204 				w = H[i][i] - p;
   4205 				r = 0.0f;
   4206 				for ( j = l; j <= n; j++ ) {
   4207 					r = r + H[i][j] * H[j][n];
   4208 				}
   4209 				if ( imaginaryEigenValues[i] < 0.0f ) {
   4210 					z = w;
   4211 					s = r;
   4212 				} else {
   4213 					l = i;
   4214 					if ( imaginaryEigenValues[i] == 0.0f ) {
   4215 						if ( w != 0.0f ) {
   4216 							H[i][n] = -r / w;
   4217 						} else {
   4218 							H[i][n] = -r / ( eps * norm );
   4219 						}
   4220 					} else {		// solve real equations
   4221 						x = H[i][i+1];
   4222 						y = H[i+1][i];
   4223 						q = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i];
   4224 						t = ( x * s - z * r ) / q;
   4225 						H[i][n] = t;
   4226 						if ( idMath::Fabs(x) > idMath::Fabs( z ) ) {
   4227 							H[i+1][n] = ( -r - w * t ) / x;
   4228 						} else {
   4229 							H[i+1][n] = ( -s - y * t ) / z;
   4230 						}
   4231 					}
   4232 
   4233 					// overflow control
   4234 					t = idMath::Fabs(H[i][n]);
   4235 					if ( ( eps * t ) * t > 1 ) {
   4236 						for ( j = i; j <= n; j++ ) {
   4237 							H[j][n] = H[j][n] / t;
   4238 						}
   4239 					}
   4240 				}
   4241 			}
   4242 		} else if ( q < 0.0f ) {	// complex vector
   4243 			int l = n-1;
   4244 
   4245 			// last vector component imaginary so matrix is triangular
   4246 			if ( idMath::Fabs( H[n][n-1] ) > idMath::Fabs( H[n-1][n] ) ) {
   4247 				H[n-1][n-1] = q / H[n][n-1];
   4248 				H[n-1][n] = -( H[n][n] - p ) / H[n][n-1];
   4249 			} else {
   4250 				ComplexDivision( 0.0f, -H[n-1][n], H[n-1][n-1]-p, q, H[n-1][n-1], H[n-1][n] );
   4251 			}
   4252 			H[n][n-1] = 0.0f;
   4253 			H[n][n] = 1.0f;
   4254 			for ( i = n-2; i >= 0; i-- ) {
   4255 				float ra, sa, vr, vi;
   4256 				ra = 0.0f;
   4257 				sa = 0.0f;
   4258 				for ( j = l; j <= n; j++ ) {
   4259 					ra = ra + H[i][j] * H[j][n-1];
   4260 					sa = sa + H[i][j] * H[j][n];
   4261 				}
   4262 				w = H[i][i] - p;
   4263 
   4264 				if ( imaginaryEigenValues[i] < 0.0f ) {
   4265 					z = w;
   4266 					r = ra;
   4267 					s = sa;
   4268 				} else {
   4269 					l = i;
   4270 					if ( imaginaryEigenValues[i] == 0.0f ) {
   4271 						ComplexDivision( -ra, -sa, w, q, H[i][n-1], H[i][n] );
   4272 					} else {
   4273 						// solve complex equations
   4274 						x = H[i][i+1];
   4275 						y = H[i+1][i];
   4276 						vr = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i] - q * q;
   4277 						vi = ( realEigenValues[i] - p ) * 2.0f * q;
   4278 						if ( vr == 0.0f && vi == 0.0f ) {
   4279 							vr = eps * norm * ( idMath::Fabs( w ) + idMath::Fabs( q ) + idMath::Fabs( x ) + idMath::Fabs( y ) + idMath::Fabs( z ) );
   4280 						}
   4281 						ComplexDivision( x * r - z * ra + q * sa, x * s - z * sa - q * ra, vr, vi, H[i][n-1], H[i][n] );
   4282 						if ( idMath::Fabs( x ) > ( idMath::Fabs( z ) + idMath::Fabs( q ) ) ) {
   4283 							H[i+1][n-1] = ( -ra - w * H[i][n-1] + q * H[i][n] ) / x;
   4284 							H[i+1][n] = ( -sa - w * H[i][n] - q * H[i][n-1] ) / x;
   4285 						} else {
   4286 							ComplexDivision( -r - y * H[i][n-1], -s - y * H[i][n], z, q, H[i+1][n-1], H[i+1][n] );
   4287 						}
   4288 					}
   4289 
   4290 					// overflow control
   4291 					t = Max( idMath::Fabs( H[i][n-1] ), idMath::Fabs( H[i][n] ) );
   4292 					if ( ( eps * t ) * t > 1 ) {
   4293 						for ( j = i; j <= n; j++ ) {
   4294 							H[j][n-1] = H[j][n-1] / t;
   4295 							H[j][n] = H[j][n] / t;
   4296 						}
   4297 					}
   4298 				}
   4299 			}
   4300 		}
   4301 	}
   4302 
   4303 	// vectors of isolated roots
   4304 	for ( i = 0; i < numRows; i++ ) {
   4305 		if ( i < low || i > high ) {
   4306 			for ( j = i; j < numRows; j++ ) {
   4307 				(*this)[i][j] = H[i][j];
   4308 			}
   4309 		}
   4310 	}
   4311 
   4312 	// back transformation to get eigenvectors of original matrix
   4313 	for ( j = numRows - 1; j >= low; j-- ) {
   4314 		for ( i = low; i <= high; i++ ) {
   4315 			z = 0.0f;
   4316 			for ( k = low; k <= Min( j, high ); k++ ) {
   4317 				z = z + (*this)[i][k] * H[k][j];
   4318 			}
   4319 			(*this)[i][j] = z;
   4320 		}
   4321 	}
   4322 
   4323 	return true;
   4324 }
   4325 
   4326 /*
   4327 ============
   4328 idMatX::Eigen_Solve
   4329 
   4330   Determine eigen values and eigen vectors for a square matrix.
   4331   The eigen values are stored in 'realEigenValues' and 'imaginaryEigenValues'.
   4332   Column i of the original matrix will store the eigen vector corresponding to the realEigenValues[i] and imaginaryEigenValues[i].
   4333 ============
   4334 */
   4335 bool idMatX::Eigen_Solve( idVecX &realEigenValues, idVecX &imaginaryEigenValues ) {
   4336     idMatX H;
   4337 
   4338 	assert( numRows == numColumns );
   4339 
   4340 	realEigenValues.SetSize( numRows );
   4341 	imaginaryEigenValues.SetSize( numRows );
   4342 
   4343 	H = *this;
   4344 
   4345     // reduce to Hessenberg form
   4346     HessenbergReduction( H );
   4347 
   4348     // reduce Hessenberg to real Schur form
   4349     return HessenbergToRealSchur( H, realEigenValues, imaginaryEigenValues );
   4350 }
   4351 
   4352 /*
   4353 ============
   4354 idMatX::Eigen_SortIncreasing
   4355 ============
   4356 */
   4357 void idMatX::Eigen_SortIncreasing( idVecX &eigenValues ) {
   4358 	for ( int i = 0, j = 0; i <= numRows - 2; i++ ) {
   4359 		j = i;
   4360 		float min = eigenValues[j];
   4361 		for ( int k = i + 1; k < numRows; k++ ) {
   4362 			if ( eigenValues[k] < min ) {
   4363 				j = k;
   4364 				min = eigenValues[j];
   4365 			}
   4366 		}
   4367 		if ( j != i ) {
   4368 			eigenValues.SwapElements( i, j );
   4369 			SwapColumns( i, j );
   4370 		}
   4371 	}
   4372 }
   4373 
   4374 /*
   4375 ============
   4376 idMatX::Eigen_SortDecreasing
   4377 ============
   4378 */
   4379 void idMatX::Eigen_SortDecreasing( idVecX &eigenValues ) {
   4380 	for ( int i = 0, j = 0; i <= numRows - 2; i++ ) {
   4381 		j = i;
   4382 		float max = eigenValues[j];
   4383 		for ( int k = i + 1; k < numRows; k++ ) {
   4384 			if ( eigenValues[k] > max ) {
   4385 				j = k;
   4386 				max = eigenValues[j];
   4387 			}
   4388 		}
   4389 		if ( j != i ) {
   4390 			eigenValues.SwapElements( i, j );
   4391 			SwapColumns( i, j );
   4392 		}
   4393 	}
   4394 }
   4395 
   4396 /*
   4397 ============
   4398 idMatX::DeterminantGeneric
   4399 ============
   4400 */
   4401 float idMatX::DeterminantGeneric() const {
   4402 	int *index;
   4403 	float det;
   4404 	idMatX tmp;
   4405 
   4406 	index = (int *) _alloca16( numRows * sizeof( int ) );
   4407 	tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
   4408 	tmp = *this;
   4409 
   4410 	if ( !tmp.LU_Factor( index, &det ) ) {
   4411 		return 0.0f;
   4412 	}
   4413 
   4414 	return det;
   4415 }
   4416 
   4417 /*
   4418 ============
   4419 idMatX::InverseSelfGeneric
   4420 ============
   4421 */
   4422 bool idMatX::InverseSelfGeneric() {
   4423 	int i, j, *index;
   4424 	idMatX tmp;
   4425 	idVecX x, b;
   4426 
   4427 	index = (int *) _alloca16( numRows * sizeof( int ) );
   4428 	tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
   4429 	tmp = *this;
   4430 
   4431 	if ( !tmp.LU_Factor( index ) ) {
   4432 		return false;
   4433 	}
   4434 
   4435 	x.SetData( numRows, VECX_ALLOCA( numRows ) );
   4436 	b.SetData( numRows, VECX_ALLOCA( numRows ) );
   4437 	b.Zero();
   4438 
   4439 	for ( i = 0; i < numRows; i++ ) {
   4440 
   4441 		b[i] = 1.0f;
   4442 		tmp.LU_Solve( x, b, index );
   4443 		for ( j = 0; j < numRows; j++ ) {
   4444 			(*this)[j][i] = x[j];
   4445 		}
   4446 		b[i] = 0.0f;
   4447 	}
   4448 	return true;
   4449 }
   4450 
   4451 /*
   4452 ============
   4453 idMatX::Test
   4454 ============
   4455 */
   4456 void idMatX::Test() {
   4457 	idMatX original, m1, m2, m3, q1, q2, r1, r2;
   4458 	idVecX v, w, u, c, d;
   4459 	int offset, size, *index1, *index2;
   4460 
   4461 	size = 6;
   4462 	original.Random( size, size, 0 );
   4463 	original = original * original.Transpose();
   4464 
   4465 	index1 = (int *) _alloca16( ( size + 1 ) * sizeof( index1[0] ) );
   4466 	index2 = (int *) _alloca16( ( size + 1 ) * sizeof( index2[0] ) );
   4467 
   4468 	/*
   4469 		idMatX::LowerTriangularInverse
   4470 	*/
   4471 
   4472 	m1 = original;
   4473 	m1.ClearUpperTriangle();
   4474 	m2 = m1;
   4475 
   4476 	m2.InverseSelf();
   4477 	m1.LowerTriangularInverse();
   4478 
   4479 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4480 		idLib::common->Warning( "idMatX::LowerTriangularInverse failed" );
   4481 	}
   4482 
   4483 	/*
   4484 		idMatX::UpperTriangularInverse
   4485 	*/
   4486 
   4487 	m1 = original;
   4488 	m1.ClearLowerTriangle();
   4489 	m2 = m1;
   4490 
   4491 	m2.InverseSelf();
   4492 	m1.UpperTriangularInverse();
   4493 
   4494 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4495 		idLib::common->Warning( "idMatX::UpperTriangularInverse failed" );
   4496 	}
   4497 
   4498 	/*
   4499 		idMatX::Inverse_GaussJordan
   4500 	*/
   4501 
   4502 	m1 = original;
   4503 
   4504 	m1.Inverse_GaussJordan();
   4505 	m1 *= original;
   4506 
   4507 	if ( !m1.IsIdentity( 1e-4f ) ) {
   4508 		idLib::common->Warning( "idMatX::Inverse_GaussJordan failed" );
   4509 	}
   4510 
   4511 	/*
   4512 		idMatX::Inverse_UpdateRankOne
   4513 	*/
   4514 
   4515 	m1 = original;
   4516 	m2 = original;
   4517 
   4518 	w.Random( size, 1 );
   4519 	v.Random( size, 2 );
   4520 
   4521 	// invert m1
   4522 	m1.Inverse_GaussJordan();
   4523 
   4524 	// modify and invert m2 
   4525 	m2.Update_RankOne( v, w, 1.0f );
   4526 	if ( !m2.Inverse_GaussJordan() ) {
   4527 		assert( 0 );
   4528 	}
   4529 
   4530 	// update inverse of m1
   4531 	m1.Inverse_UpdateRankOne( v, w, 1.0f );
   4532 
   4533 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4534 		idLib::common->Warning( "idMatX::Inverse_UpdateRankOne failed" );
   4535 	}
   4536 
   4537 	/*
   4538 		idMatX::Inverse_UpdateRowColumn
   4539 	*/
   4540 
   4541 	for ( offset = 0; offset < size; offset++ ) {
   4542 		m1 = original;
   4543 		m2 = original;
   4544 
   4545 		v.Random( size, 1 );
   4546 		w.Random( size, 2 );
   4547 		w[offset] = 0.0f;
   4548 
   4549 		// invert m1
   4550 		m1.Inverse_GaussJordan();
   4551 
   4552 		// modify and invert m2
   4553 		m2.Update_RowColumn( v, w, offset );
   4554 		if ( !m2.Inverse_GaussJordan() ) {
   4555 			assert( 0 );
   4556 		}
   4557 
   4558 		// update inverse of m1
   4559 		m1.Inverse_UpdateRowColumn( v, w, offset );
   4560 
   4561 		if ( !m1.Compare( m2, 1e-3f ) ) {
   4562 			idLib::common->Warning( "idMatX::Inverse_UpdateRowColumn failed" );
   4563 		}
   4564 	}
   4565 
   4566 	/*
   4567 		idMatX::Inverse_UpdateIncrement
   4568 	*/
   4569 
   4570 	m1 = original;
   4571 	m2 = original;
   4572 
   4573 	v.Random( size + 1, 1 );
   4574 	w.Random( size + 1, 2 );
   4575 	w[size] = 0.0f;
   4576 
   4577 	// invert m1
   4578 	m1.Inverse_GaussJordan();
   4579 
   4580 	// modify and invert m2 
   4581 	m2.Update_Increment( v, w );
   4582 	if ( !m2.Inverse_GaussJordan() ) {
   4583 		assert( 0 );
   4584 	}
   4585 
   4586 	// update inverse of m1
   4587 	m1.Inverse_UpdateIncrement( v, w );
   4588 
   4589 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4590 		idLib::common->Warning( "idMatX::Inverse_UpdateIncrement failed" );
   4591 	}
   4592 
   4593 	/*
   4594 		idMatX::Inverse_UpdateDecrement
   4595 	*/
   4596 
   4597 	for ( offset = 0; offset < size; offset++ ) {
   4598 		m1 = original;
   4599 		m2 = original;
   4600 
   4601 		v.SetSize( 6 );
   4602 		w.SetSize( 6 );
   4603 		for ( int i = 0; i < size; i++ ) {
   4604 			v[i] = original[i][offset];
   4605 			w[i] = original[offset][i];
   4606 		}
   4607 
   4608 		// invert m1
   4609 		m1.Inverse_GaussJordan();
   4610 
   4611 		// modify and invert m2
   4612 		m2.Update_Decrement( offset );
   4613 		if ( !m2.Inverse_GaussJordan() ) {
   4614 			assert( 0 );
   4615 		}
   4616 
   4617 		// update inverse of m1
   4618 		m1.Inverse_UpdateDecrement( v, w, offset );
   4619 
   4620 		if ( !m1.Compare( m2, 1e-3f ) ) {
   4621 			idLib::common->Warning( "idMatX::Inverse_UpdateDecrement failed" );
   4622 		}
   4623 	}
   4624 
   4625 	/*
   4626 		idMatX::LU_Factor
   4627 	*/
   4628 
   4629 	m1 = original;
   4630 
   4631 	m1.LU_Factor( NULL );	// no pivoting
   4632 	m1.LU_UnpackFactors( m2, m3 );
   4633 	m1 = m2 * m3;
   4634 
   4635 	if ( !original.Compare( m1, 1e-4f ) ) {
   4636 		idLib::common->Warning( "idMatX::LU_Factor failed" );
   4637 	}
   4638 
   4639 	/*
   4640 		idMatX::LU_UpdateRankOne
   4641 	*/
   4642 
   4643 	m1 = original;
   4644 	m2 = original;
   4645 
   4646 	w.Random( size, 1 );
   4647 	v.Random( size, 2 );
   4648 
   4649 	// factor m1
   4650 	m1.LU_Factor( index1 );
   4651 
   4652 	// modify and factor m2 
   4653 	m2.Update_RankOne( v, w, 1.0f );
   4654 	if ( !m2.LU_Factor( index2 ) ) {
   4655 		assert( 0 );
   4656 	}
   4657 	m2.LU_MultiplyFactors( m3, index2 );
   4658 	m2 = m3;
   4659 
   4660 	// update factored m1
   4661 	m1.LU_UpdateRankOne( v, w, 1.0f, index1 );
   4662 	m1.LU_MultiplyFactors( m3, index1 );
   4663 	m1 = m3;
   4664 
   4665 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4666 		idLib::common->Warning( "idMatX::LU_UpdateRankOne failed" );
   4667 	}
   4668 
   4669 	/*
   4670 		idMatX::LU_UpdateRowColumn
   4671 	*/
   4672 
   4673 	for ( offset = 0; offset < size; offset++ ) {
   4674 		m1 = original;
   4675 		m2 = original;
   4676 
   4677 		v.Random( size, 1 );
   4678 		w.Random( size, 2 );
   4679 		w[offset] = 0.0f;
   4680 
   4681 		// factor m1
   4682 		m1.LU_Factor( index1 );
   4683 
   4684 		// modify and factor m2
   4685 		m2.Update_RowColumn( v, w, offset );
   4686 		if ( !m2.LU_Factor( index2 ) ) {
   4687 			assert( 0 );
   4688 		}
   4689 		m2.LU_MultiplyFactors( m3, index2 );
   4690 		m2 = m3;
   4691 
   4692 		// update m1
   4693 		m1.LU_UpdateRowColumn( v, w, offset, index1  );
   4694 		m1.LU_MultiplyFactors( m3, index1 );
   4695 		m1 = m3;
   4696 
   4697 		if ( !m1.Compare( m2, 1e-3f ) ) {
   4698 			idLib::common->Warning( "idMatX::LU_UpdateRowColumn failed" );
   4699 		}
   4700 	}
   4701 
   4702 	/*
   4703 		idMatX::LU_UpdateIncrement
   4704 	*/
   4705 
   4706 	m1 = original;
   4707 	m2 = original;
   4708 
   4709 	v.Random( size + 1, 1 );
   4710 	w.Random( size + 1, 2 );
   4711 	w[size] = 0.0f;
   4712 
   4713 	// factor m1
   4714 	m1.LU_Factor( index1 );
   4715 
   4716 	// modify and factor m2 
   4717 	m2.Update_Increment( v, w );
   4718 	if ( !m2.LU_Factor( index2 ) ) {
   4719 		assert( 0 );
   4720 	}
   4721 	m2.LU_MultiplyFactors( m3, index2 );
   4722 	m2 = m3;
   4723 
   4724 	// update factored m1
   4725 	m1.LU_UpdateIncrement( v, w, index1 );
   4726 	m1.LU_MultiplyFactors( m3, index1 );
   4727 	m1 = m3;
   4728 
   4729 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4730 		idLib::common->Warning( "idMatX::LU_UpdateIncrement failed" );
   4731 	}
   4732 
   4733 	/*
   4734 		idMatX::LU_UpdateDecrement
   4735 	*/
   4736 
   4737 	for ( offset = 0; offset < size; offset++ ) {
   4738 		m1 = original;
   4739 		m2 = original;
   4740 
   4741 		v.SetSize( 6 );
   4742 		w.SetSize( 6 );
   4743 		for ( int i = 0; i < size; i++ ) {
   4744 			v[i] = original[i][offset];
   4745 			w[i] = original[offset][i];
   4746 		}
   4747 
   4748 		// factor m1
   4749 		m1.LU_Factor( index1 );
   4750 
   4751 		// modify and factor m2
   4752 		m2.Update_Decrement( offset );
   4753 		if ( !m2.LU_Factor( index2 ) ) {
   4754 			assert( 0 );
   4755 		}
   4756 		m2.LU_MultiplyFactors( m3, index2 );
   4757 		m2 = m3;
   4758 
   4759 		u.SetSize( 6 );
   4760 		for ( int i = 0; i < size; i++ ) {
   4761 			u[i] = original[index1[offset]][i];
   4762 		}
   4763 
   4764 		// update factors of m1
   4765 		m1.LU_UpdateDecrement( v, w, u, offset, index1 );
   4766 		m1.LU_MultiplyFactors( m3, index1 );
   4767 		m1 = m3;
   4768 
   4769 		if ( !m1.Compare( m2, 1e-3f ) ) {
   4770 			idLib::common->Warning( "idMatX::LU_UpdateDecrement failed" );
   4771 		}
   4772 	}
   4773 
   4774 	/*
   4775 		idMatX::LU_Inverse
   4776 	*/
   4777 
   4778 	m2 = original;
   4779 
   4780 	m2.LU_Factor( NULL );
   4781 	m2.LU_Inverse( m1, NULL );
   4782 	m1 *= original;
   4783 
   4784 	if ( !m1.IsIdentity( 1e-4f ) ) {
   4785 		idLib::common->Warning( "idMatX::LU_Inverse failed" );
   4786 	}
   4787 
   4788 	/*
   4789 		idMatX::QR_Factor
   4790 	*/
   4791 
   4792 	c.SetSize( size );
   4793 	d.SetSize( size );
   4794 
   4795 	m1 = original;
   4796 
   4797 	m1.QR_Factor( c, d );
   4798 	m1.QR_UnpackFactors( q1, r1, c, d );
   4799 	m1 = q1 * r1;
   4800 
   4801 	if ( !original.Compare( m1, 1e-4f ) ) {
   4802 		idLib::common->Warning( "idMatX::QR_Factor failed" );
   4803 	}
   4804 
   4805 	/*
   4806 		idMatX::QR_UpdateRankOne
   4807 	*/
   4808 
   4809 	c.SetSize( size );
   4810 	d.SetSize( size );
   4811 
   4812 	m1 = original;
   4813 	m2 = original;
   4814 
   4815 	w.Random( size, 0 );
   4816 	v = w;
   4817 
   4818 	// factor m1
   4819 	m1.QR_Factor( c, d );
   4820 	m1.QR_UnpackFactors( q1, r1, c, d );
   4821 
   4822 	// modify and factor m2 
   4823 	m2.Update_RankOne( v, w, 1.0f );
   4824 	if ( !m2.QR_Factor( c, d ) ) {
   4825 		assert( 0 );
   4826 	}
   4827 	m2.QR_UnpackFactors( q2, r2, c, d );
   4828 	m2 = q2 * r2;
   4829 
   4830 	// update factored m1
   4831 	q1.QR_UpdateRankOne( r1, v, w, 1.0f );
   4832 	m1 = q1 * r1;
   4833 
   4834 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4835 		idLib::common->Warning( "idMatX::QR_UpdateRankOne failed" );
   4836 	}
   4837 
   4838 	/*
   4839 		idMatX::QR_UpdateRowColumn
   4840 	*/
   4841 
   4842 	for ( offset = 0; offset < size; offset++ ) {
   4843 		c.SetSize( size );
   4844 		d.SetSize( size );
   4845 
   4846 		m1 = original;
   4847 		m2 = original;
   4848 
   4849 		v.Random( size, 1 );
   4850 		w.Random( size, 2 );
   4851 		w[offset] = 0.0f;
   4852 
   4853 		// factor m1
   4854 		m1.QR_Factor( c, d );
   4855 		m1.QR_UnpackFactors( q1, r1, c, d );
   4856 
   4857 		// modify and factor m2
   4858 		m2.Update_RowColumn( v, w, offset );
   4859 		if ( !m2.QR_Factor( c, d ) ) {
   4860 			assert( 0 );
   4861 		}
   4862 		m2.QR_UnpackFactors( q2, r2, c, d );
   4863 		m2 = q2 * r2;
   4864 
   4865 		// update m1
   4866 		q1.QR_UpdateRowColumn( r1, v, w, offset );
   4867 		m1 = q1 * r1;
   4868 
   4869 		if ( !m1.Compare( m2, 1e-3f ) ) {
   4870 			idLib::common->Warning( "idMatX::QR_UpdateRowColumn failed" );
   4871 		}
   4872 	}
   4873 
   4874 	/*
   4875 		idMatX::QR_UpdateIncrement
   4876 	*/
   4877 
   4878 	c.SetSize( size+1 );
   4879 	d.SetSize( size+1 );
   4880 
   4881 	m1 = original;
   4882 	m2 = original;
   4883 
   4884 	v.Random( size + 1, 1 );
   4885 	w.Random( size + 1, 2 );
   4886 	w[size] = 0.0f;
   4887 
   4888 	// factor m1
   4889 	m1.QR_Factor( c, d );
   4890 	m1.QR_UnpackFactors( q1, r1, c, d );
   4891 
   4892 	// modify and factor m2 
   4893 	m2.Update_Increment( v, w );
   4894 	if ( !m2.QR_Factor( c, d ) ) {
   4895 		assert( 0 );
   4896 	}
   4897 	m2.QR_UnpackFactors( q2, r2, c, d );
   4898 	m2 = q2 * r2;
   4899 
   4900 	// update factored m1
   4901 	q1.QR_UpdateIncrement( r1, v, w );
   4902 	m1 = q1 * r1;
   4903 
   4904 	if ( !m1.Compare( m2, 1e-4f ) ) {
   4905 		idLib::common->Warning( "idMatX::QR_UpdateIncrement failed" );
   4906 	}
   4907 
   4908 	/*
   4909 		idMatX::QR_UpdateDecrement
   4910 	*/
   4911 
   4912 	for ( offset = 0; offset < size; offset++ ) {
   4913 		c.SetSize( size+1 );
   4914 		d.SetSize( size+1 );
   4915 
   4916 		m1 = original;
   4917 		m2 = original;
   4918 
   4919 		v.SetSize( 6 );
   4920 		w.SetSize( 6 );
   4921 		for ( int i = 0; i < size; i++ ) {
   4922 			v[i] = original[i][offset];
   4923 			w[i] = original[offset][i];
   4924 		}
   4925 
   4926 		// factor m1
   4927 		m1.QR_Factor( c, d );
   4928 		m1.QR_UnpackFactors( q1, r1, c, d );
   4929 
   4930 		// modify and factor m2
   4931 		m2.Update_Decrement( offset );
   4932 		if ( !m2.QR_Factor( c, d ) ) {
   4933 			assert( 0 );
   4934 		}
   4935 		m2.QR_UnpackFactors( q2, r2, c, d );
   4936 		m2 = q2 * r2;
   4937 
   4938 		// update factors of m1
   4939 		q1.QR_UpdateDecrement( r1, v, w, offset );
   4940 		m1 = q1 * r1;
   4941 
   4942 		if ( !m1.Compare( m2, 1e-3f ) ) {
   4943 			idLib::common->Warning( "idMatX::QR_UpdateDecrement failed" );
   4944 		}
   4945 	}
   4946 
   4947 	/*
   4948 		idMatX::QR_Inverse
   4949 	*/
   4950 
   4951 	m2 = original;
   4952 
   4953 	m2.QR_Factor( c, d );
   4954 	m2.QR_Inverse( m1, c, d );
   4955 	m1 *= original;
   4956 
   4957 	if ( !m1.IsIdentity( 1e-4f ) ) {
   4958 		idLib::common->Warning( "idMatX::QR_Inverse failed" );
   4959 	}
   4960 
   4961 	/*
   4962 		idMatX::SVD_Factor
   4963 	*/
   4964 
   4965 	m1 = original;
   4966 	m3.Zero( size, size );
   4967 	w.Zero( size );
   4968 
   4969 	m1.SVD_Factor( w, m3 );
   4970 	m2.Diag( w );
   4971 	m3.TransposeSelf();
   4972 	m1 = m1 * m2 * m3;
   4973 
   4974 	if ( !original.Compare( m1, 1e-4f ) ) {
   4975 		idLib::common->Warning( "idMatX::SVD_Factor failed" );
   4976 	}
   4977 
   4978 	/*
   4979 		idMatX::SVD_Inverse
   4980 	*/
   4981 
   4982 	m2 = original;
   4983 
   4984 	m2.SVD_Factor( w, m3 );
   4985 	m2.SVD_Inverse( m1, w, m3 );
   4986 	m1 *= original;
   4987 
   4988 	if ( !m1.IsIdentity( 1e-4f ) ) {
   4989 		idLib::common->Warning( "idMatX::SVD_Inverse failed" );
   4990 	}
   4991 
   4992 	/*
   4993 		idMatX::Cholesky_Factor
   4994 	*/
   4995 
   4996 	m1 = original;
   4997 
   4998 	m1.Cholesky_Factor();
   4999 	m1.Cholesky_MultiplyFactors( m2 );
   5000 
   5001 	if ( !original.Compare( m2, 1e-4f ) ) {
   5002 		idLib::common->Warning( "idMatX::Cholesky_Factor failed" );
   5003 	}
   5004 
   5005 	/*
   5006 		idMatX::Cholesky_UpdateRankOne
   5007 	*/
   5008 
   5009 	m1 = original;
   5010 	m2 = original;
   5011 
   5012 	w.Random( size, 0 );
   5013 
   5014 	// factor m1
   5015 	m1.Cholesky_Factor();
   5016 	m1.ClearUpperTriangle();
   5017 
   5018 	// modify and factor m2 
   5019 	m2.Update_RankOneSymmetric( w, 1.0f );
   5020 	if ( !m2.Cholesky_Factor() ) {
   5021 		assert( 0 );
   5022 	}
   5023 	m2.ClearUpperTriangle();
   5024 
   5025 	// update factored m1
   5026 	m1.Cholesky_UpdateRankOne( w, 1.0f, 0 );
   5027 
   5028 	if ( !m1.Compare( m2, 1e-4f ) ) {
   5029 		idLib::common->Warning( "idMatX::Cholesky_UpdateRankOne failed" );
   5030 	}
   5031 
   5032 	/*
   5033 		idMatX::Cholesky_UpdateRowColumn
   5034 	*/
   5035 
   5036 	for ( offset = 0; offset < size; offset++ ) {
   5037 		m1 = original;
   5038 		m2 = original;
   5039 
   5040 		// factor m1
   5041 		m1.Cholesky_Factor();
   5042 		m1.ClearUpperTriangle();
   5043 
   5044 		int pdtable[] = { 1, 0, 1, 0, 0, 0 };
   5045 		w.Random( size, pdtable[offset] );
   5046 		w *= 0.1f;
   5047 
   5048 		// modify and factor m2
   5049 		m2.Update_RowColumnSymmetric( w, offset );
   5050 		if ( !m2.Cholesky_Factor() ) {
   5051 			assert( 0 );
   5052 		}
   5053 		m2.ClearUpperTriangle();
   5054 
   5055 		// update m1
   5056 		m1.Cholesky_UpdateRowColumn( w, offset );
   5057 
   5058 		if ( !m1.Compare( m2, 1e-3f ) ) {
   5059 			idLib::common->Warning( "idMatX::Cholesky_UpdateRowColumn failed" );
   5060 		}
   5061 	}
   5062 
   5063 	/*
   5064 		idMatX::Cholesky_UpdateIncrement
   5065 	*/
   5066 
   5067 	m1.Random( size + 1, size + 1, 0 );
   5068 	m3 = m1 * m1.Transpose();
   5069 
   5070 	m1.SquareSubMatrix( m3, size );
   5071 	m2 = m1;
   5072 
   5073 	w.SetSize( size + 1 );
   5074 	for ( int i = 0; i < size + 1; i++ ) {
   5075 		w[i] = m3[size][i];
   5076 	}
   5077 
   5078 	// factor m1
   5079 	m1.Cholesky_Factor();
   5080 
   5081 	// modify and factor m2 
   5082 	m2.Update_IncrementSymmetric( w );
   5083 	if ( !m2.Cholesky_Factor() ) {
   5084 		assert( 0 );
   5085 	}
   5086 
   5087 	// update factored m1
   5088 	m1.Cholesky_UpdateIncrement( w );
   5089 
   5090 	m1.ClearUpperTriangle();
   5091 	m2.ClearUpperTriangle();
   5092 
   5093 	if ( !m1.Compare( m2, 1e-4f ) ) {
   5094 		idLib::common->Warning( "idMatX::Cholesky_UpdateIncrement failed" );
   5095 	}
   5096 
   5097 	/*
   5098 		idMatX::Cholesky_UpdateDecrement
   5099 	*/
   5100 
   5101 	for ( offset = 0; offset < size; offset += size - 1 ) {
   5102 		m1 = original;
   5103 		m2 = original;
   5104 
   5105 		v.SetSize( 6 );
   5106 		for ( int i = 0; i < size; i++ ) {
   5107 			v[i] = original[i][offset];
   5108 		}
   5109 
   5110 		// factor m1
   5111 		m1.Cholesky_Factor();
   5112 
   5113 		// modify and factor m2
   5114 		m2.Update_Decrement( offset );
   5115 		if ( !m2.Cholesky_Factor() ) {
   5116 			assert( 0 );
   5117 		}
   5118 
   5119 		// update factors of m1
   5120 		m1.Cholesky_UpdateDecrement( v, offset );
   5121 
   5122 		if ( !m1.Compare( m2, 1e-3f ) ) {
   5123 			idLib::common->Warning( "idMatX::Cholesky_UpdateDecrement failed" );
   5124 		}
   5125 	}
   5126 
   5127 	/*
   5128 		idMatX::Cholesky_Inverse
   5129 	*/
   5130 
   5131 	m2 = original;
   5132 
   5133 	m2.Cholesky_Factor();
   5134 	m2.Cholesky_Inverse( m1 );
   5135 	m1 *= original;
   5136 
   5137 	if ( !m1.IsIdentity( 1e-4f ) ) {
   5138 		idLib::common->Warning( "idMatX::Cholesky_Inverse failed" );
   5139 	}
   5140 
   5141 	/*
   5142 		idMatX::LDLT_Factor
   5143 	*/
   5144 
   5145 	m1 = original;
   5146 
   5147 	m1.LDLT_Factor();
   5148 	m1.LDLT_MultiplyFactors( m2 );
   5149 
   5150 	if ( !original.Compare( m2, 1e-4f ) ) {
   5151 		idLib::common->Warning( "idMatX::LDLT_Factor failed" );
   5152 	}
   5153 
   5154 	m1.LDLT_UnpackFactors( m2, m3 );
   5155 	m2 = m2 * m3 * m2.Transpose();
   5156 
   5157 	if ( !original.Compare( m2, 1e-4f ) ) {
   5158 		idLib::common->Warning( "idMatX::LDLT_Factor failed" );
   5159 	}
   5160 
   5161 	/*
   5162 		idMatX::LDLT_UpdateRankOne
   5163 	*/
   5164 
   5165 	m1 = original;
   5166 	m2 = original;
   5167 
   5168 	w.Random( size, 0 );
   5169 
   5170 	// factor m1
   5171 	m1.LDLT_Factor();
   5172 	m1.ClearUpperTriangle();
   5173 
   5174 	// modify and factor m2 
   5175 	m2.Update_RankOneSymmetric( w, 1.0f );
   5176 	if ( !m2.LDLT_Factor() ) {
   5177 		assert( 0 );
   5178 	}
   5179 	m2.ClearUpperTriangle();
   5180 
   5181 	// update factored m1
   5182 	m1.LDLT_UpdateRankOne( w, 1.0f, 0 );
   5183 
   5184 	if ( !m1.Compare( m2, 1e-4f ) ) {
   5185 		idLib::common->Warning( "idMatX::LDLT_UpdateRankOne failed" );
   5186 	}
   5187 
   5188 	/*
   5189 		idMatX::LDLT_UpdateRowColumn
   5190 	*/
   5191 
   5192 	for ( offset = 0; offset < size; offset++ ) {
   5193 		m1 = original;
   5194 		m2 = original;
   5195 
   5196 		w.Random( size, 0 );
   5197 
   5198 		// factor m1
   5199 		m1.LDLT_Factor();
   5200 		m1.ClearUpperTriangle();
   5201 
   5202 		// modify and factor m2
   5203 		m2.Update_RowColumnSymmetric( w, offset );
   5204 		if ( !m2.LDLT_Factor() ) {
   5205 			assert( 0 );
   5206 		}
   5207 		m2.ClearUpperTriangle();
   5208 
   5209 		// update m1
   5210 		m1.LDLT_UpdateRowColumn( w, offset );
   5211 
   5212 		if ( !m1.Compare( m2, 1e-3f ) ) {
   5213 			idLib::common->Warning( "idMatX::LDLT_UpdateRowColumn failed" );
   5214 		}
   5215 	}
   5216 
   5217 	/*
   5218 		idMatX::LDLT_UpdateIncrement
   5219 	*/
   5220 
   5221 	m1.Random( size + 1, size + 1, 0 );
   5222 	m3 = m1 * m1.Transpose();
   5223 
   5224 	m1.SquareSubMatrix( m3, size );
   5225 	m2 = m1;
   5226 
   5227 	w.SetSize( size + 1 );
   5228 	for ( int i = 0; i < size + 1; i++ ) {
   5229 		w[i] = m3[size][i];
   5230 	}
   5231 
   5232 	// factor m1
   5233 	m1.LDLT_Factor();
   5234 
   5235 	// modify and factor m2 
   5236 	m2.Update_IncrementSymmetric( w );
   5237 	if ( !m2.LDLT_Factor() ) {
   5238 		assert( 0 );
   5239 	}
   5240 
   5241 	// update factored m1
   5242 	m1.LDLT_UpdateIncrement( w );
   5243 
   5244 	m1.ClearUpperTriangle();
   5245 	m2.ClearUpperTriangle();
   5246 
   5247 	if ( !m1.Compare( m2, 1e-4f ) ) {
   5248 		idLib::common->Warning( "idMatX::LDLT_UpdateIncrement failed" );
   5249 	}
   5250 
   5251 	/*
   5252 		idMatX::LDLT_UpdateDecrement
   5253 	*/
   5254 
   5255 	for ( offset = 0; offset < size; offset++ ) {
   5256 		m1 = original;
   5257 		m2 = original;
   5258 
   5259 		v.SetSize( 6 );
   5260 		for ( int i = 0; i < size; i++ ) {
   5261 			v[i] = original[i][offset];
   5262 		}
   5263 
   5264 		// factor m1
   5265 		m1.LDLT_Factor();
   5266 
   5267 		// modify and factor m2
   5268 		m2.Update_Decrement( offset );
   5269 		if ( !m2.LDLT_Factor() ) {
   5270 			assert( 0 );
   5271 		}
   5272 
   5273 		// update factors of m1
   5274 		m1.LDLT_UpdateDecrement( v, offset );
   5275 
   5276 		if ( !m1.Compare( m2, 1e-3f ) ) {
   5277 			idLib::common->Warning( "idMatX::LDLT_UpdateDecrement failed" );
   5278 		}
   5279 	}
   5280 
   5281 	/*
   5282 		idMatX::LDLT_Inverse
   5283 	*/
   5284 
   5285 	m2 = original;
   5286 
   5287 	m2.LDLT_Factor();
   5288 	m2.LDLT_Inverse( m1 );
   5289 	m1 *= original;
   5290 
   5291 	if ( !m1.IsIdentity( 1e-4f ) ) {
   5292 		idLib::common->Warning( "idMatX::LDLT_Inverse failed" );
   5293 	}
   5294 
   5295 	/*
   5296 		idMatX::Eigen_SolveSymmetricTriDiagonal
   5297 	*/
   5298 
   5299 	m3 = original;
   5300 	m3.TriDiagonal_ClearTriangles();
   5301 	m1 = m3;
   5302 
   5303 	v.SetSize( size );
   5304 
   5305 	m1.Eigen_SolveSymmetricTriDiagonal( v );
   5306 
   5307 	m3.TransposeMultiply( m2, m1 );
   5308 
   5309 	for ( int i = 0; i < size; i++ ) {
   5310 		for ( int j = 0; j < size; j++ ) {
   5311 			m1[i][j] *= v[j];
   5312 		}
   5313 	}
   5314 
   5315 	if ( !m1.Compare( m2, 1e-4f ) ) {
   5316 		idLib::common->Warning( "idMatX::Eigen_SolveSymmetricTriDiagonal failed" );
   5317 	}
   5318 
   5319 	/*
   5320 		idMatX::Eigen_SolveSymmetric
   5321 	*/
   5322 
   5323 	m3 = original;
   5324 	m1 = m3;
   5325 
   5326 	v.SetSize( size );
   5327 
   5328 	m1.Eigen_SolveSymmetric( v );
   5329 
   5330 	m3.TransposeMultiply( m2, m1 );
   5331 
   5332 	for ( int i = 0; i < size; i++ ) {
   5333 		for ( int j = 0; j < size; j++ ) {
   5334 			m1[i][j] *= v[j];
   5335 		}
   5336 	}
   5337 
   5338 	if ( !m1.Compare( m2, 1e-4f ) ) {
   5339 		idLib::common->Warning( "idMatX::Eigen_SolveSymmetric failed" );
   5340 	}
   5341 
   5342 	/*
   5343 		idMatX::Eigen_Solve
   5344 	*/
   5345 
   5346 	m3 = original;
   5347 	m1 = m3;
   5348 
   5349 	v.SetSize( size );
   5350 	w.SetSize( size );
   5351 
   5352 	m1.Eigen_Solve( v, w );
   5353 
   5354 	m3.TransposeMultiply( m2, m1 );
   5355 
   5356 	for ( int i = 0; i < size; i++ ) {
   5357 		for ( int j = 0; j < size; j++ ) {
   5358 			m1[i][j] *= v[j];
   5359 		}
   5360 	}
   5361 
   5362 	if ( !m1.Compare( m2, 1e-4f ) ) {
   5363 		idLib::common->Warning( "idMatX::Eigen_Solve failed" );
   5364 	}
   5365 }