mirror of
https://github.com/id-Software/DOOM-3-BFG.git
synced 2024-12-11 21:21:27 +00:00
6148 lines
115 KiB
C++
6148 lines
115 KiB
C++
/*
|
|
===========================================================================
|
|
|
|
Doom 3 BFG Edition GPL Source Code
|
|
Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.
|
|
Copyright (C) 2012 Robert Beckebans
|
|
|
|
This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").
|
|
|
|
Doom 3 BFG Edition Source Code is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
Doom 3 BFG Edition Source Code is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with Doom 3 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
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.
|
|
|
|
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.
|
|
|
|
===========================================================================
|
|
*/
|
|
|
|
#pragma hdrstop
|
|
#include "precompiled.h"
|
|
|
|
//===============================================================
|
|
//
|
|
// idMatX
|
|
//
|
|
//===============================================================
|
|
|
|
float idMatX::temp[MATX_MAX_TEMP + 4];
|
|
// RB: changed int to intptr_t
|
|
float* idMatX::tempPtr = ( float* )( ( ( intptr_t ) idMatX::temp + 15 ) & ~15 );
|
|
// RB end
|
|
int idMatX::tempIndex = 0;
|
|
|
|
|
|
/*
|
|
============
|
|
idMatX::ChangeSize
|
|
============
|
|
*/
|
|
void idMatX::ChangeSize( int rows, int columns, bool makeZero )
|
|
{
|
|
int alloc = ( rows * columns + 3 ) & ~3;
|
|
if( alloc > alloced && alloced != -1 )
|
|
{
|
|
float* oldMat = mat;
|
|
mat = ( float* ) Mem_Alloc16( alloc * sizeof( float ), TAG_MATH );
|
|
if( makeZero )
|
|
{
|
|
memset( mat, 0, alloc * sizeof( float ) );
|
|
}
|
|
alloced = alloc;
|
|
if( oldMat )
|
|
{
|
|
int minRow = Min( numRows, rows );
|
|
int minColumn = Min( numColumns, columns );
|
|
for( int i = 0; i < minRow; i++ )
|
|
{
|
|
for( int j = 0; j < minColumn; j++ )
|
|
{
|
|
mat[ i * columns + j ] = oldMat[ i * numColumns + j ];
|
|
}
|
|
}
|
|
Mem_Free16( oldMat );
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if( columns < numColumns )
|
|
{
|
|
int minRow = Min( numRows, rows );
|
|
for( int i = 0; i < minRow; i++ )
|
|
{
|
|
for( int j = 0; j < columns; j++ )
|
|
{
|
|
mat[ i * columns + j ] = mat[ i * numColumns + j ];
|
|
}
|
|
}
|
|
}
|
|
else if( columns > numColumns )
|
|
{
|
|
for( int i = Min( numRows, rows ) - 1; i >= 0; i-- )
|
|
{
|
|
if( makeZero )
|
|
{
|
|
for( int j = columns - 1; j >= numColumns; j-- )
|
|
{
|
|
mat[ i * columns + j ] = 0.0f;
|
|
}
|
|
}
|
|
for( int j = numColumns - 1; j >= 0; j-- )
|
|
{
|
|
mat[ i * columns + j ] = mat[ i * numColumns + j ];
|
|
}
|
|
}
|
|
}
|
|
if( makeZero && rows > numRows )
|
|
{
|
|
memset( mat + numRows * columns, 0, ( rows - numRows ) * columns * sizeof( float ) );
|
|
}
|
|
}
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::RemoveRow
|
|
============
|
|
*/
|
|
idMatX& idMatX::RemoveRow( int r )
|
|
{
|
|
int i;
|
|
|
|
assert( r < numRows );
|
|
|
|
numRows--;
|
|
|
|
for( i = r; i < numRows; i++ )
|
|
{
|
|
memcpy( &mat[i * numColumns], &mat[( i + 1 ) * numColumns], numColumns * sizeof( float ) );
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::RemoveColumn
|
|
============
|
|
*/
|
|
idMatX& idMatX::RemoveColumn( int r )
|
|
{
|
|
int i;
|
|
|
|
assert( r < numColumns );
|
|
|
|
numColumns--;
|
|
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
|
|
}
|
|
memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::RemoveRowColumn
|
|
============
|
|
*/
|
|
idMatX& idMatX::RemoveRowColumn( int r )
|
|
{
|
|
int i;
|
|
|
|
assert( r < numRows && r < numColumns );
|
|
|
|
numRows--;
|
|
numColumns--;
|
|
|
|
if( r > 0 )
|
|
{
|
|
for( i = 0; i < r - 1; i++ )
|
|
{
|
|
memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
|
|
}
|
|
memmove( &mat[i * numColumns + r], &mat[i * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
|
|
}
|
|
|
|
memcpy( &mat[r * numColumns], &mat[( r + 1 ) * ( numColumns + 1 )], r * sizeof( float ) );
|
|
|
|
for( i = r; i < numRows - 1; i++ )
|
|
{
|
|
memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], numColumns * sizeof( float ) );
|
|
}
|
|
memcpy( &mat[i * numColumns + r], &mat[( i + 1 ) * ( numColumns + 1 ) + r + 1], ( numColumns - r ) * sizeof( float ) );
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
========================
|
|
idMatX::CopyLowerToUpperTriangle
|
|
========================
|
|
*/
|
|
void idMatX::CopyLowerToUpperTriangle()
|
|
{
|
|
assert( ( GetNumColumns() & 3 ) == 0 );
|
|
assert( GetNumColumns() >= GetNumRows() );
|
|
|
|
|
|
const int n = GetNumColumns();
|
|
const int m = GetNumRows();
|
|
|
|
const int n0 = 0;
|
|
const int n1 = n;
|
|
const int n2 = ( n << 1 );
|
|
const int n3 = ( n << 1 ) + n;
|
|
const int n4 = ( n << 2 );
|
|
|
|
const int b1 = ( ( m - 0 ) >> 1 ) & 1; // ( m & 3 ) > 1
|
|
const int b2 = ( ( m - 1 ) >> 1 ) & 1; // ( m & 3 ) > 2 (provided ( m & 3 ) > 0)
|
|
|
|
const int n1_masked = ( n & -b1 );
|
|
const int n2_masked = ( n & -b1 ) + ( n & -b2 );
|
|
|
|
const __m128 mask0 = __m128c( _mm_set_epi32( 0, 0, 0, -1 ) );
|
|
const __m128 mask1 = __m128c( _mm_set_epi32( 0, 0, -1, -1 ) );
|
|
const __m128 mask2 = __m128c( _mm_set_epi32( 0, -1, -1, -1 ) );
|
|
const __m128 mask3 = __m128c( _mm_set_epi32( -1, -1, -1, -1 ) );
|
|
|
|
const __m128 bottomMask[2] = { __m128c( _mm_set1_epi32( 0 ) ), __m128c( _mm_set1_epi32( -1 ) ) };
|
|
|
|
float* __restrict basePtr = ToFloatPtr();
|
|
|
|
for( int i = 0; i < m - 3; i += 4 )
|
|
{
|
|
|
|
// copy top left diagonal 4x4 block elements
|
|
__m128 r0 = _mm_and_ps( _mm_load_ps( basePtr + n0 ), mask0 );
|
|
__m128 r1 = _mm_and_ps( _mm_load_ps( basePtr + n1 ), mask1 );
|
|
__m128 r2 = _mm_and_ps( _mm_load_ps( basePtr + n2 ), mask2 );
|
|
__m128 r3 = _mm_and_ps( _mm_load_ps( basePtr + n3 ), mask3 );
|
|
|
|
__m128 t0 = _mm_unpacklo_ps( r0, r2 ); // x0, z0, x1, z1
|
|
__m128 t1 = _mm_unpackhi_ps( r0, r2 ); // x2, z2, x3, z3
|
|
__m128 t2 = _mm_unpacklo_ps( r1, r3 ); // y0, w0, y1, w1
|
|
__m128 t3 = _mm_unpackhi_ps( r1, r3 ); // y2, w2, y3, w3
|
|
|
|
__m128 s0 = _mm_unpacklo_ps( t0, t2 ); // x0, y0, z0, w0
|
|
__m128 s1 = _mm_unpackhi_ps( t0, t2 ); // x1, y1, z1, w1
|
|
__m128 s2 = _mm_unpacklo_ps( t1, t3 ); // x2, y2, z2, w2
|
|
__m128 s3 = _mm_unpackhi_ps( t1, t3 ); // x3, y3, z3, w3
|
|
|
|
r0 = _mm_or_ps( r0, s0 );
|
|
r1 = _mm_or_ps( r1, s1 );
|
|
r2 = _mm_or_ps( r2, s2 );
|
|
r3 = _mm_or_ps( r3, s3 );
|
|
|
|
_mm_store_ps( basePtr + n0, r0 );
|
|
_mm_store_ps( basePtr + n1, r1 );
|
|
_mm_store_ps( basePtr + n2, r2 );
|
|
_mm_store_ps( basePtr + n3, r3 );
|
|
|
|
// copy one column of 4x4 blocks to one row of 4x4 blocks
|
|
const float* __restrict srcPtr = basePtr;
|
|
float* __restrict dstPtr = basePtr;
|
|
|
|
for( int j = i + 4; j < m - 3; j += 4 )
|
|
{
|
|
srcPtr += n4;
|
|
dstPtr += 4;
|
|
|
|
__m128 r0 = _mm_load_ps( srcPtr + n0 );
|
|
__m128 r1 = _mm_load_ps( srcPtr + n1 );
|
|
__m128 r2 = _mm_load_ps( srcPtr + n2 );
|
|
__m128 r3 = _mm_load_ps( srcPtr + n3 );
|
|
|
|
__m128 t0 = _mm_unpacklo_ps( r0, r2 ); // x0, z0, x1, z1
|
|
__m128 t1 = _mm_unpackhi_ps( r0, r2 ); // x2, z2, x3, z3
|
|
__m128 t2 = _mm_unpacklo_ps( r1, r3 ); // y0, w0, y1, w1
|
|
__m128 t3 = _mm_unpackhi_ps( r1, r3 ); // y2, w2, y3, w3
|
|
|
|
r0 = _mm_unpacklo_ps( t0, t2 ); // x0, y0, z0, w0
|
|
r1 = _mm_unpackhi_ps( t0, t2 ); // x1, y1, z1, w1
|
|
r2 = _mm_unpacklo_ps( t1, t3 ); // x2, y2, z2, w2
|
|
r3 = _mm_unpackhi_ps( t1, t3 ); // x3, y3, z3, w3
|
|
|
|
_mm_store_ps( dstPtr + n0, r0 );
|
|
_mm_store_ps( dstPtr + n1, r1 );
|
|
_mm_store_ps( dstPtr + n2, r2 );
|
|
_mm_store_ps( dstPtr + n3, r3 );
|
|
}
|
|
|
|
// copy the last partial 4x4 block elements
|
|
if( m & 3 )
|
|
{
|
|
srcPtr += n4;
|
|
dstPtr += 4;
|
|
|
|
__m128 r0 = _mm_load_ps( srcPtr + n0 );
|
|
__m128 r1 = _mm_and_ps( _mm_load_ps( srcPtr + n1_masked ), bottomMask[b1] );
|
|
__m128 r2 = _mm_and_ps( _mm_load_ps( srcPtr + n2_masked ), bottomMask[b2] );
|
|
__m128 r3 = _mm_setzero_ps();
|
|
|
|
__m128 t0 = _mm_unpacklo_ps( r0, r2 ); // x0, z0, x1, z1
|
|
__m128 t1 = _mm_unpackhi_ps( r0, r2 ); // x2, z2, x3, z3
|
|
__m128 t2 = _mm_unpacklo_ps( r1, r3 ); // y0, w0, y1, w1
|
|
__m128 t3 = _mm_unpackhi_ps( r1, r3 ); // y2, w2, y3, w3
|
|
|
|
r0 = _mm_unpacklo_ps( t0, t2 ); // x0, y0, z0, w0
|
|
r1 = _mm_unpackhi_ps( t0, t2 ); // x1, y1, z1, w1
|
|
r2 = _mm_unpacklo_ps( t1, t3 ); // x2, y2, z2, w2
|
|
r3 = _mm_unpackhi_ps( t1, t3 ); // x3, y3, z3, w3
|
|
|
|
_mm_store_ps( dstPtr + n0, r0 );
|
|
_mm_store_ps( dstPtr + n1, r1 );
|
|
_mm_store_ps( dstPtr + n2, r2 );
|
|
_mm_store_ps( dstPtr + n3, r3 );
|
|
}
|
|
|
|
basePtr += n4 + 4;
|
|
}
|
|
|
|
// copy the lower right partial diagonal 4x4 block elements
|
|
if( m & 3 )
|
|
{
|
|
__m128 r0 = _mm_and_ps( _mm_load_ps( basePtr + n0 ), mask0 );
|
|
__m128 r1 = _mm_and_ps( _mm_load_ps( basePtr + n1_masked ), _mm_and_ps( mask1, bottomMask[b1] ) );
|
|
__m128 r2 = _mm_and_ps( _mm_load_ps( basePtr + n2_masked ), _mm_and_ps( mask2, bottomMask[b2] ) );
|
|
__m128 r3 = _mm_setzero_ps();
|
|
|
|
__m128 t0 = _mm_unpacklo_ps( r0, r2 ); // x0, z0, x1, z1
|
|
__m128 t1 = _mm_unpackhi_ps( r0, r2 ); // x2, z2, x3, z3
|
|
__m128 t2 = _mm_unpacklo_ps( r1, r3 ); // y0, w0, y1, w1
|
|
__m128 t3 = _mm_unpackhi_ps( r1, r3 ); // y2, w2, y3, w3
|
|
|
|
__m128 s0 = _mm_unpacklo_ps( t0, t2 ); // x0, y0, z0, w0
|
|
__m128 s1 = _mm_unpackhi_ps( t0, t2 ); // x1, y1, z1, w1
|
|
__m128 s2 = _mm_unpacklo_ps( t1, t3 ); // x2, y2, z2, w2
|
|
|
|
r0 = _mm_or_ps( r0, s0 );
|
|
r1 = _mm_or_ps( r1, s1 );
|
|
r2 = _mm_or_ps( r2, s2 );
|
|
|
|
_mm_store_ps( basePtr + n2_masked, r2 );
|
|
_mm_store_ps( basePtr + n1_masked, r1 );
|
|
_mm_store_ps( basePtr + n0, r0 );
|
|
}
|
|
|
|
|
|
#ifdef _DEBUG
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numRows; j++ )
|
|
{
|
|
assert( mat[ i * numColumns + j ] == mat[ j * numColumns + i ] );
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
/*
|
|
============
|
|
idMatX::IsOrthogonal
|
|
|
|
returns true if (*this) * this->Transpose() == Identity
|
|
============
|
|
*/
|
|
bool idMatX::IsOrthogonal( const float epsilon ) const
|
|
{
|
|
float* ptr1, *ptr2, sum;
|
|
|
|
if( !IsSquare() )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
ptr1 = mat;
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
ptr2 = mat + j;
|
|
sum = ptr1[0] * ptr2[0] - ( float )( i == j );
|
|
for( int n = 1; n < numColumns; n++ )
|
|
{
|
|
ptr2 += numColumns;
|
|
sum += ptr1[n] * ptr2[0];
|
|
}
|
|
if( idMath::Fabs( sum ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
ptr1 += numColumns;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsOrthonormal
|
|
|
|
returns true if (*this) * this->Transpose() == Identity and the length of each column vector is 1
|
|
============
|
|
*/
|
|
bool idMatX::IsOrthonormal( const float epsilon ) const
|
|
{
|
|
float* ptr1, *ptr2, sum;
|
|
|
|
if( !IsSquare() )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
ptr1 = mat;
|
|
for( int i = 0; i < numRows; i++ )
|
|
{
|
|
for( int j = 0; j < numColumns; j++ )
|
|
{
|
|
ptr2 = mat + j;
|
|
sum = ptr1[0] * ptr2[0] - ( float )( i == j );
|
|
for( int n = 1; n < numColumns; n++ )
|
|
{
|
|
ptr2 += numColumns;
|
|
sum += ptr1[n] * ptr2[0];
|
|
}
|
|
if( idMath::Fabs( sum ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
ptr1 += numColumns;
|
|
|
|
ptr2 = mat + i;
|
|
sum = ptr2[0] * ptr2[0] - 1.0f;
|
|
for( int j = 1; j < numRows; j++ )
|
|
{
|
|
ptr2 += numColumns;
|
|
sum += ptr2[j] * ptr2[j];
|
|
}
|
|
if( idMath::Fabs( sum ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsPMatrix
|
|
|
|
returns true if the matrix is a P-matrix
|
|
A square matrix is a P-matrix if all its principal minors are positive.
|
|
============
|
|
*/
|
|
bool idMatX::IsPMatrix( const float epsilon ) const
|
|
{
|
|
int i, j;
|
|
float d;
|
|
idMatX m;
|
|
|
|
if( !IsSquare() )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if( numRows <= 0 )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
if( ( *this )[0][0] <= epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if( numRows <= 1 )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
m.SetData( numRows - 1, numColumns - 1, MATX_ALLOCA( ( numRows - 1 ) * ( numColumns - 1 ) ) );
|
|
|
|
for( i = 1; i < numRows; i++ )
|
|
{
|
|
for( j = 1; j < numColumns; j++ )
|
|
{
|
|
m[i - 1][j - 1] = ( *this )[i][j];
|
|
}
|
|
}
|
|
|
|
if( !m.IsPMatrix( epsilon ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
for( i = 1; i < numRows; i++ )
|
|
{
|
|
d = ( *this )[i][0] / ( *this )[0][0];
|
|
for( j = 1; j < numColumns; j++ )
|
|
{
|
|
m[i - 1][j - 1] = ( *this )[i][j] - d * ( *this )[0][j];
|
|
}
|
|
}
|
|
|
|
if( !m.IsPMatrix( epsilon ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsZMatrix
|
|
|
|
returns true if the matrix is a Z-matrix
|
|
A square matrix M is a Z-matrix if M[i][j] <= 0 for all i != j.
|
|
============
|
|
*/
|
|
bool idMatX::IsZMatrix( const float epsilon ) const
|
|
{
|
|
int i, j;
|
|
|
|
if( !IsSquare() )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
if( ( *this )[i][j] > epsilon && i != j )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsPositiveDefinite
|
|
|
|
returns true if the matrix is Positive Definite (PD)
|
|
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.
|
|
============
|
|
*/
|
|
bool idMatX::IsPositiveDefinite( const float epsilon ) const
|
|
{
|
|
int i, j, k;
|
|
float d, s;
|
|
idMatX m;
|
|
|
|
// the matrix must be square
|
|
if( !IsSquare() )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// copy matrix
|
|
m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
|
|
m = *this;
|
|
|
|
// add transpose
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
m[i][j] += ( *this )[j][i];
|
|
}
|
|
}
|
|
|
|
// test Positive Definiteness with Gaussian pivot steps
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
for( j = i; j < numColumns; j++ )
|
|
{
|
|
if( m[j][j] <= epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
d = 1.0f / m[i][i];
|
|
for( j = i + 1; j < numColumns; j++ )
|
|
{
|
|
s = d * m[j][i];
|
|
m[j][i] = 0.0f;
|
|
for( k = i + 1; k < numRows; k++ )
|
|
{
|
|
m[j][k] -= s * m[i][k];
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsSymmetricPositiveDefinite
|
|
|
|
returns true if the matrix is Symmetric Positive Definite (PD)
|
|
============
|
|
*/
|
|
bool idMatX::IsSymmetricPositiveDefinite( const float epsilon ) const
|
|
{
|
|
idMatX m;
|
|
|
|
// the matrix must be symmetric
|
|
if( !IsSymmetric( epsilon ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// copy matrix
|
|
m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
|
|
m = *this;
|
|
|
|
// being able to obtain Cholesky factors is both a necessary and sufficient condition for positive definiteness
|
|
return m.Cholesky_Factor();
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsPositiveSemiDefinite
|
|
|
|
returns true if the matrix is Positive Semi Definite (PSD)
|
|
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.
|
|
============
|
|
*/
|
|
bool idMatX::IsPositiveSemiDefinite( const float epsilon ) const
|
|
{
|
|
int i, j, k;
|
|
float d, s;
|
|
idMatX m;
|
|
|
|
// the matrix must be square
|
|
if( !IsSquare() )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// copy original matrix
|
|
m.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
|
|
m = *this;
|
|
|
|
// add transpose
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
m[i][j] += ( *this )[j][i];
|
|
}
|
|
}
|
|
|
|
// test Positive Semi Definiteness with Gaussian pivot steps
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
for( j = i; j < numColumns; j++ )
|
|
{
|
|
if( m[j][j] < -epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
if( m[j][j] > epsilon )
|
|
{
|
|
continue;
|
|
}
|
|
for( k = 0; k < numRows; k++ )
|
|
{
|
|
if( idMath::Fabs( m[k][j] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
if( idMath::Fabs( m[j][k] ) > epsilon )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
if( m[i][i] <= epsilon )
|
|
{
|
|
continue;
|
|
}
|
|
|
|
d = 1.0f / m[i][i];
|
|
for( j = i + 1; j < numColumns; j++ )
|
|
{
|
|
s = d * m[j][i];
|
|
m[j][i] = 0.0f;
|
|
for( k = i + 1; k < numRows; k++ )
|
|
{
|
|
m[j][k] -= s * m[i][k];
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsSymmetricPositiveSemiDefinite
|
|
|
|
returns true if the matrix is Symmetric Positive Semi Definite (PSD)
|
|
============
|
|
*/
|
|
bool idMatX::IsSymmetricPositiveSemiDefinite( const float epsilon ) const
|
|
{
|
|
|
|
// the matrix must be symmetric
|
|
if( !IsSymmetric( epsilon ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
return IsPositiveSemiDefinite( epsilon );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LowerTriangularInverse
|
|
|
|
in-place inversion of the lower triangular matrix
|
|
============
|
|
*/
|
|
bool idMatX::LowerTriangularInverse()
|
|
{
|
|
int i, j, k;
|
|
double d, sum;
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
d = ( *this )[i][i];
|
|
if( d == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
( *this )[i][i] = d = 1.0f / d;
|
|
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( k = j; k < i; k++ )
|
|
{
|
|
sum -= ( *this )[i][k] * ( *this )[k][j];
|
|
}
|
|
( *this )[i][j] = sum * d;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::UpperTriangularInverse
|
|
|
|
in-place inversion of the upper triangular matrix
|
|
============
|
|
*/
|
|
bool idMatX::UpperTriangularInverse()
|
|
{
|
|
int i, j, k;
|
|
double d, sum;
|
|
|
|
for( i = numRows - 1; i >= 0; i-- )
|
|
{
|
|
d = ( *this )[i][i];
|
|
if( d == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
( *this )[i][i] = d = 1.0f / d;
|
|
|
|
for( j = numRows - 1; j > i; j-- )
|
|
{
|
|
sum = 0.0f;
|
|
for( k = j; k > i; k-- )
|
|
{
|
|
sum -= ( *this )[i][k] * ( *this )[k][j];
|
|
}
|
|
( *this )[i][j] = sum * d;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idMatX::ToString
|
|
=============
|
|
*/
|
|
const char* idMatX::ToString( int precision ) const
|
|
{
|
|
return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RankOne
|
|
|
|
Updates the matrix to obtain the matrix: A + alpha * v * w'
|
|
============
|
|
*/
|
|
void idMatX::Update_RankOne( const idVecX& v, const idVecX& w, float alpha )
|
|
{
|
|
int i, j;
|
|
float s;
|
|
|
|
assert( v.GetSize() >= numRows );
|
|
assert( w.GetSize() >= numColumns );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
s = alpha * v[i];
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
( *this )[i][j] += s * w[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RankOneSymmetric
|
|
|
|
Updates the matrix to obtain the matrix: A + alpha * v * v'
|
|
============
|
|
*/
|
|
void idMatX::Update_RankOneSymmetric( const idVecX& v, float alpha )
|
|
{
|
|
int i, j;
|
|
float s;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
s = alpha * v[i];
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
( *this )[i][j] += s * v[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RowColumn
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
A + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
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]
|
|
============
|
|
*/
|
|
void idMatX::Update_RowColumn( const idVecX& v, const idVecX& w, int r )
|
|
{
|
|
int i;
|
|
|
|
assert( w[r] == 0.0f );
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
( *this )[i][r] += v[i];
|
|
}
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
( *this )[r][i] += w[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RowColumnSymmetric
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
A + [ a b c ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
|
|
============
|
|
*/
|
|
void idMatX::Update_RowColumnSymmetric( const idVecX& v, int r )
|
|
{
|
|
int i;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
|
|
for( i = 0; i < r; i++ )
|
|
{
|
|
( *this )[i][r] += v[i];
|
|
( *this )[r][i] += v[i];
|
|
}
|
|
( *this )[r][r] += v[r];
|
|
for( i = r + 1; i < numRows; i++ )
|
|
{
|
|
( *this )[i][r] += v[i];
|
|
( *this )[r][i] += v[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_Increment
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1]], w[numColumns] = 0
|
|
============
|
|
*/
|
|
void idMatX::Update_Increment( const idVecX& v, const idVecX& w )
|
|
{
|
|
int i;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows + 1 );
|
|
assert( w.GetSize() >= numColumns + 1 );
|
|
|
|
ChangeSize( numRows + 1, numColumns + 1, false );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
( *this )[i][numColumns - 1] = v[i];
|
|
}
|
|
for( i = 0; i < numColumns - 1; i++ )
|
|
{
|
|
( *this )[numRows - 1][i] = w[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_IncrementSymmetric
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ A a ]
|
|
[ a b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows]
|
|
============
|
|
*/
|
|
void idMatX::Update_IncrementSymmetric( const idVecX& v )
|
|
{
|
|
int i;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows + 1 );
|
|
|
|
ChangeSize( numRows + 1, numColumns + 1, false );
|
|
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
( *this )[i][numColumns - 1] = v[i];
|
|
}
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
( *this )[numRows - 1][i] = v[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_Decrement
|
|
|
|
Updates the matrix to obtain a matrix with row r and column r removed.
|
|
============
|
|
*/
|
|
void idMatX::Update_Decrement( int r )
|
|
{
|
|
RemoveRowColumn( r );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_GaussJordan
|
|
|
|
in-place inversion using Gauss-Jordan elimination
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_GaussJordan()
|
|
{
|
|
int i, j, k, r, c;
|
|
float d, max;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
int* columnIndex = ( int* ) _alloca16( numRows * sizeof( int ) );
|
|
int* rowIndex = ( int* ) _alloca16( numRows * sizeof( int ) );
|
|
bool* pivot = ( bool* ) _alloca16( numRows * sizeof( bool ) );
|
|
|
|
memset( pivot, 0, numRows * sizeof( bool ) );
|
|
|
|
// elimination with full pivoting
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
// search the whole matrix except for pivoted rows for the maximum absolute value
|
|
max = 0.0f;
|
|
r = c = 0;
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
if( !pivot[j] )
|
|
{
|
|
for( k = 0; k < numRows; k++ )
|
|
{
|
|
if( !pivot[k] )
|
|
{
|
|
d = idMath::Fabs( ( *this )[j][k] );
|
|
if( d > max )
|
|
{
|
|
max = d;
|
|
r = j;
|
|
c = k;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if( max == 0.0f )
|
|
{
|
|
// matrix is not invertible
|
|
return false;
|
|
}
|
|
|
|
pivot[c] = true;
|
|
|
|
// swap rows such that entry (c,c) has the pivot entry
|
|
if( r != c )
|
|
{
|
|
SwapRows( r, c );
|
|
}
|
|
|
|
// keep track of the row permutation
|
|
rowIndex[i] = r;
|
|
columnIndex[i] = c;
|
|
|
|
// scale the row to make the pivot entry equal to 1
|
|
d = 1.0f / ( *this )[c][c];
|
|
( *this )[c][c] = 1.0f;
|
|
for( k = 0; k < numRows; k++ )
|
|
{
|
|
( *this )[c][k] *= d;
|
|
}
|
|
|
|
// zero out the pivot column entries in the other rows
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
if( j != c )
|
|
{
|
|
d = ( *this )[j][c];
|
|
( *this )[j][c] = 0.0f;
|
|
for( k = 0; k < numRows; k++ )
|
|
{
|
|
( *this )[j][k] -= ( *this )[c][k] * d;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// reorder rows to store the inverse of the original matrix
|
|
for( j = numRows - 1; j >= 0; j-- )
|
|
{
|
|
if( rowIndex[j] != columnIndex[j] )
|
|
{
|
|
for( k = 0; k < numRows; k++ )
|
|
{
|
|
d = ( *this )[k][rowIndex[j]];
|
|
( *this )[k][rowIndex[j]] = ( *this )[k][columnIndex[j]];
|
|
( *this )[k][columnIndex[j]] = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateRankOne
|
|
|
|
Updates the in-place inverse using the Sherman-Morrison formula to obtain the inverse for the matrix: A + alpha * v * w'
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateRankOne( const idVecX& v, const idVecX& w, float alpha )
|
|
{
|
|
int i, j;
|
|
float beta, s;
|
|
idVecX y, z;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
|
|
y.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
z.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
Multiply( y, v );
|
|
TransposeMultiply( z, w );
|
|
beta = 1.0f + ( w * y );
|
|
|
|
if( beta == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
alpha /= beta;
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
s = y[i] * alpha;
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
( *this )[i][j] -= s * z[j];
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateRowColumn
|
|
|
|
Updates the in-place inverse to obtain the inverse for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
A + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
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]
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateRowColumn( const idVecX& v, const idVecX& w, int r )
|
|
{
|
|
idVecX s;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows && r < numColumns );
|
|
assert( w[r] == 0.0f );
|
|
|
|
s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
|
|
s.Zero();
|
|
s[r] = 1.0f;
|
|
|
|
if( !Inverse_UpdateRankOne( v, s, 1.0f ) )
|
|
{
|
|
return false;
|
|
}
|
|
if( !Inverse_UpdateRankOne( s, w, 1.0f ) )
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateIncrement
|
|
|
|
Updates the in-place inverse to obtain the inverse for the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateIncrement( const idVecX& v, const idVecX& w )
|
|
{
|
|
idVecX v2;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows + 1 );
|
|
assert( w.GetSize() >= numColumns + 1 );
|
|
|
|
ChangeSize( numRows + 1, numColumns + 1, true );
|
|
( *this )[numRows - 1][numRows - 1] = 1.0f;
|
|
|
|
v2.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
v2 = v;
|
|
v2[numRows - 1] -= 1.0f;
|
|
|
|
return Inverse_UpdateRowColumn( v2, w, numRows - 1 );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateDecrement
|
|
|
|
Updates the in-place inverse to obtain the inverse of the matrix with row r and column r removed.
|
|
v and w should store the column and row of the original matrix respectively.
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateDecrement( const idVecX& v, const idVecX& w, int r )
|
|
{
|
|
idVecX v1, w1;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( w.GetSize() >= numColumns );
|
|
assert( r >= 0 && r < numRows && r < numColumns );
|
|
|
|
v1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
w1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
w1 = -w;
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if( !Inverse_UpdateRowColumn( v1, w1, r ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement( r );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_Solve
|
|
|
|
Solve Ax = b with A inverted
|
|
============
|
|
*/
|
|
void idMatX::Inverse_Solve( idVecX& x, const idVecX& b ) const
|
|
{
|
|
Multiply( x, b );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_Factor
|
|
|
|
in-place factorization: LU
|
|
L is a triangular matrix stored in the lower triangle.
|
|
L has ones on the diagonal that are not stored.
|
|
U is a triangular matrix stored in the upper triangle.
|
|
If index != NULL partial pivoting is used for numerical stability.
|
|
If index != NULL it must point to an array of numRow integers and is used to keep track of the row permutation.
|
|
If det != NULL the determinant of the matrix is calculated and stored.
|
|
============
|
|
*/
|
|
bool idMatX::LU_Factor( int* index, float* det )
|
|
{
|
|
int i, j, k, newi, min;
|
|
double s, t, d, w;
|
|
|
|
// if partial pivoting should be used
|
|
if( index )
|
|
{
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
index[i] = i;
|
|
}
|
|
}
|
|
|
|
w = 1.0f;
|
|
min = Min( numRows, numColumns );
|
|
for( i = 0; i < min; i++ )
|
|
{
|
|
|
|
newi = i;
|
|
s = idMath::Fabs( ( *this )[i][i] );
|
|
|
|
if( index )
|
|
{
|
|
// find the largest absolute pivot
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
t = idMath::Fabs( ( *this )[j][i] );
|
|
if( t > s )
|
|
{
|
|
newi = j;
|
|
s = t;
|
|
}
|
|
}
|
|
}
|
|
|
|
if( s == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if( newi != i && index )
|
|
{
|
|
|
|
w = -w;
|
|
|
|
// swap index elements
|
|
k = index[i];
|
|
index[i] = index[newi];
|
|
index[newi] = k;
|
|
|
|
// swap rows
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
t = ( *this )[newi][j];
|
|
( *this )[newi][j] = ( *this )[i][j];
|
|
( *this )[i][j] = t;
|
|
}
|
|
}
|
|
|
|
if( i < numRows )
|
|
{
|
|
d = 1.0f / ( *this )[i][i];
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
( *this )[j][i] *= d;
|
|
}
|
|
}
|
|
|
|
if( i < min - 1 )
|
|
{
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
d = ( *this )[j][i];
|
|
for( k = i + 1; k < numColumns; k++ )
|
|
{
|
|
( *this )[j][k] -= d * ( *this )[i][k];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if( det )
|
|
{
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
w *= ( *this )[i][i];
|
|
}
|
|
*det = w;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateRankOne
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix: LU + alpha * v * w'
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateRankOne( const idVecX& v, const idVecX& w, float alpha, int* index )
|
|
{
|
|
int i, j, max;
|
|
float* y, *z;
|
|
double diag, beta, p0, p1, d;
|
|
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
|
|
y = ( float* ) _alloca16( v.GetSize() * sizeof( float ) );
|
|
z = ( float* ) _alloca16( w.GetSize() * sizeof( float ) );
|
|
|
|
if( index != NULL )
|
|
{
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
y[i] = alpha * v[index[i]];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
y[i] = alpha * v[i];
|
|
}
|
|
}
|
|
|
|
memcpy( z, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
|
|
|
|
max = Min( numRows, numColumns );
|
|
for( i = 0; i < max; i++ )
|
|
{
|
|
diag = ( *this )[i][i];
|
|
|
|
p0 = y[i];
|
|
p1 = z[i];
|
|
diag += p0 * p1;
|
|
|
|
if( diag == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
beta = p1 / diag;
|
|
|
|
( *this )[i][i] = diag;
|
|
|
|
for( j = i + 1; j < numColumns; j++ )
|
|
{
|
|
|
|
d = ( *this )[i][j];
|
|
|
|
d += p0 * z[j];
|
|
z[j] -= beta * d;
|
|
|
|
( *this )[i][j] = d;
|
|
}
|
|
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
|
|
d = ( *this )[j][i];
|
|
|
|
y[j] -= p0 * d;
|
|
d += beta * y[j];
|
|
|
|
( *this )[j][i] = d;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateRowColumn
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
LU + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
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]
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateRowColumn( const idVecX& v, const idVecX& w, int r, int* index )
|
|
{
|
|
#if 0
|
|
|
|
idVecX s;
|
|
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows && r < numColumns );
|
|
assert( w[r] == 0.0f );
|
|
|
|
s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
|
|
s.Zero();
|
|
s[r] = 1.0f;
|
|
|
|
if( !LU_UpdateRankOne( v, s, 1.0f, index ) )
|
|
{
|
|
return false;
|
|
}
|
|
if( !LU_UpdateRankOne( s, w, 1.0f, index ) )
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
#else
|
|
|
|
int i, j, min, max, rp;
|
|
float* y0, *y1, *z0, *z1;
|
|
double diag, beta0, beta1, p0, p1, q0, q1, d;
|
|
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numColumns && r < numRows );
|
|
assert( w[r] == 0.0f );
|
|
|
|
y0 = ( float* ) _alloca16( v.GetSize() * sizeof( float ) );
|
|
z0 = ( float* ) _alloca16( w.GetSize() * sizeof( float ) );
|
|
y1 = ( float* ) _alloca16( v.GetSize() * sizeof( float ) );
|
|
z1 = ( float* ) _alloca16( w.GetSize() * sizeof( float ) );
|
|
|
|
if( index != NULL )
|
|
{
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
y0[i] = v[index[i]];
|
|
}
|
|
rp = r;
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
if( index[i] == r )
|
|
{
|
|
rp = i;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
memcpy( y0, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
|
|
rp = r;
|
|
}
|
|
|
|
memset( y1, 0, v.GetSize() * sizeof( float ) );
|
|
y1[rp] = 1.0f;
|
|
|
|
memset( z0, 0, w.GetSize() * sizeof( float ) );
|
|
z0[r] = 1.0f;
|
|
|
|
memcpy( z1, w.ToFloatPtr(), w.GetSize() * sizeof( float ) );
|
|
|
|
// update the beginning of the to be updated row and column
|
|
min = Min( r, rp );
|
|
for( i = 0; i < min; i++ )
|
|
{
|
|
p0 = y0[i];
|
|
beta1 = z1[i] / ( *this )[i][i];
|
|
|
|
( *this )[i][r] += p0;
|
|
for( j = i + 1; j < numColumns; j++ )
|
|
{
|
|
z1[j] -= beta1 * ( *this )[i][j];
|
|
}
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
y0[j] -= p0 * ( *this )[j][i];
|
|
}
|
|
( *this )[rp][i] += beta1;
|
|
}
|
|
|
|
// update the lower right corner starting at r,r
|
|
max = Min( numRows, numColumns );
|
|
for( i = min; i < max; i++ )
|
|
{
|
|
diag = ( *this )[i][i];
|
|
|
|
p0 = y0[i];
|
|
p1 = z0[i];
|
|
diag += p0 * p1;
|
|
|
|
if( diag == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
beta0 = p1 / diag;
|
|
|
|
q0 = y1[i];
|
|
q1 = z1[i];
|
|
diag += q0 * q1;
|
|
|
|
if( diag == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
beta1 = q1 / diag;
|
|
|
|
( *this )[i][i] = diag;
|
|
|
|
for( j = i + 1; j < numColumns; j++ )
|
|
{
|
|
|
|
d = ( *this )[i][j];
|
|
|
|
d += p0 * z0[j];
|
|
z0[j] -= beta0 * d;
|
|
|
|
d += q0 * z1[j];
|
|
z1[j] -= beta1 * d;
|
|
|
|
( *this )[i][j] = d;
|
|
}
|
|
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
|
|
d = ( *this )[j][i];
|
|
|
|
y0[j] -= p0 * d;
|
|
d += beta0 * y0[j];
|
|
|
|
y1[j] -= q0 * d;
|
|
d += beta1 * y1[j];
|
|
|
|
( *this )[j][i] = d;
|
|
}
|
|
}
|
|
return true;
|
|
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateIncrement
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateIncrement( const idVecX& v, const idVecX& w, int* index )
|
|
{
|
|
int i, j;
|
|
float sum;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows + 1 );
|
|
assert( w.GetSize() >= numColumns + 1 );
|
|
|
|
ChangeSize( numRows + 1, numColumns + 1, true );
|
|
|
|
// add row to L
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
sum = w[i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[numRows - 1][j] * ( *this )[j][i];
|
|
}
|
|
( *this )[numRows - 1 ][i] = sum / ( *this )[i][i];
|
|
}
|
|
|
|
// add row to the permutation index
|
|
if( index != NULL )
|
|
{
|
|
index[numRows - 1] = numRows - 1;
|
|
}
|
|
|
|
// add column to U
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
if( index != NULL )
|
|
{
|
|
sum = v[index[i]];
|
|
}
|
|
else
|
|
{
|
|
sum = v[i];
|
|
}
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * ( *this )[j][numRows - 1];
|
|
}
|
|
( *this )[i][numRows - 1] = sum;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateDecrement
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v and w should store the column and row of the original matrix respectively.
|
|
If index != NULL then u should store row index[r] of the original matrix. If index == NULL then u = w.
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateDecrement( const idVecX& v, const idVecX& w, const idVecX& u, int r, int* index )
|
|
{
|
|
int i, p;
|
|
idVecX v1, w1;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows && r < numColumns );
|
|
|
|
v1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
w1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
if( index != NULL )
|
|
{
|
|
|
|
// find the pivot row
|
|
for( p = i = 0; i < numRows; i++ )
|
|
{
|
|
if( index[i] == r )
|
|
{
|
|
p = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
w1 = -u;
|
|
|
|
if( p != r )
|
|
{
|
|
SwapValues( v1[index[r]], v1[index[p]] );
|
|
SwapValues( index[r], index[p] );
|
|
}
|
|
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if( !LU_UpdateRowColumn( v1, w1, r, index ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if( p != r )
|
|
{
|
|
|
|
if( idMath::Fabs( u[p] ) < 1e-4f )
|
|
{
|
|
// NOTE: an additional row interchange is required for numerical stability
|
|
}
|
|
|
|
// move row index[r] of the original matrix to row index[p] of the original matrix
|
|
v1.Zero();
|
|
v1[index[p]] = 1.0f;
|
|
w1 = u - w;
|
|
|
|
if( !LU_UpdateRankOne( v1, w1, 1.0f, index ) )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// remove the row from the permutation index
|
|
for( i = r; i < numRows - 1; i++ )
|
|
{
|
|
index[i] = index[i + 1];
|
|
}
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
if( index[i] > r )
|
|
{
|
|
index[i]--;
|
|
}
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
v1 = -v;
|
|
w1 = -w;
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if( !LU_UpdateRowColumn( v1, w1, r, index ) )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement( r );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: LU
|
|
============
|
|
*/
|
|
void idMatX::LU_Solve( idVecX& x, const idVecX& b, const int* index ) const
|
|
{
|
|
int i, j;
|
|
double sum;
|
|
|
|
assert( x.GetSize() == numColumns && b.GetSize() == numRows );
|
|
|
|
// solve L
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
if( index != NULL )
|
|
{
|
|
sum = b[index[i]];
|
|
}
|
|
else
|
|
{
|
|
sum = b[i];
|
|
}
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
|
|
// solve U
|
|
for( i = numRows - 1; i >= 0; i-- )
|
|
{
|
|
sum = x[i];
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * x[j];
|
|
}
|
|
x[i] = sum / ( *this )[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as LU
|
|
============
|
|
*/
|
|
void idMatX::LU_Inverse( idMatX& inv, const int* index ) const
|
|
{
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
x.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.Zero();
|
|
inv.SetSize( numRows, numColumns );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
b[i] = 1.0f;
|
|
LU_Solve( x, b, index );
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UnpackFactors
|
|
|
|
Unpacks the in-place LU factorization.
|
|
============
|
|
*/
|
|
void idMatX::LU_UnpackFactors( idMatX& L, idMatX& U ) const
|
|
{
|
|
int i, j;
|
|
|
|
L.Zero( numRows, numColumns );
|
|
U.Zero( numRows, numColumns );
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
L[i][j] = ( *this )[i][j];
|
|
}
|
|
L[i][i] = 1.0f;
|
|
for( j = i; j < numColumns; j++ )
|
|
{
|
|
U[i][j] = ( *this )[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place LU factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::LU_MultiplyFactors( idMatX& m, const int* index ) const
|
|
{
|
|
int r, rp, i, j;
|
|
double sum;
|
|
|
|
m.SetSize( numRows, numColumns );
|
|
|
|
for( r = 0; r < numRows; r++ )
|
|
{
|
|
|
|
if( index != NULL )
|
|
{
|
|
rp = index[r];
|
|
}
|
|
else
|
|
{
|
|
rp = r;
|
|
}
|
|
|
|
// calculate row of matrix
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
if( i >= r )
|
|
{
|
|
sum = ( *this )[r][i];
|
|
}
|
|
else
|
|
{
|
|
sum = 0.0f;
|
|
}
|
|
for( j = 0; j <= i && j < r; j++ )
|
|
{
|
|
sum += ( *this )[r][j] * ( *this )[j][i];
|
|
}
|
|
m[rp][i] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Factor
|
|
|
|
in-place factorization: QR
|
|
Q is an orthogonal matrix represented as a product of Householder matrices stored in the lower triangle and c.
|
|
R is a triangular matrix stored in the upper triangle except for the diagonal elements which are stored in d.
|
|
The initial matrix has to be square.
|
|
============
|
|
*/
|
|
bool idMatX::QR_Factor( idVecX& c, idVecX& d )
|
|
{
|
|
int i, j, k;
|
|
double scale, s, t, sum;
|
|
bool singular = false;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
|
|
|
|
for( k = 0; k < numRows - 1; k++ )
|
|
{
|
|
|
|
scale = 0.0f;
|
|
for( i = k; i < numRows; i++ )
|
|
{
|
|
s = idMath::Fabs( ( *this )[i][k] );
|
|
if( s > scale )
|
|
{
|
|
scale = s;
|
|
}
|
|
}
|
|
if( scale == 0.0f )
|
|
{
|
|
singular = true;
|
|
c[k] = d[k] = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
|
|
s = 1.0f / scale;
|
|
for( i = k; i < numRows; i++ )
|
|
{
|
|
( *this )[i][k] *= s;
|
|
}
|
|
|
|
sum = 0.0f;
|
|
for( i = k; i < numRows; i++ )
|
|
{
|
|
s = ( *this )[i][k];
|
|
sum += s * s;
|
|
}
|
|
|
|
s = idMath::Sqrt( sum );
|
|
if( ( *this )[k][k] < 0.0f )
|
|
{
|
|
s = -s;
|
|
}
|
|
( *this )[k][k] += s;
|
|
c[k] = s * ( *this )[k][k];
|
|
d[k] = -scale * s;
|
|
|
|
for( j = k + 1; j < numRows; j++ )
|
|
{
|
|
|
|
sum = 0.0f;
|
|
for( i = k; i < numRows; i++ )
|
|
{
|
|
sum += ( *this )[i][k] * ( *this )[i][j];
|
|
}
|
|
t = sum / c[k];
|
|
for( i = k; i < numRows; i++ )
|
|
{
|
|
( *this )[i][j] -= t * ( *this )[i][k];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
d[numRows - 1] = ( *this )[( numRows - 1 ) ][( numRows - 1 ) ];
|
|
if( d[numRows - 1] == 0.0f )
|
|
{
|
|
singular = true;
|
|
}
|
|
|
|
return !singular;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Rotate
|
|
|
|
Performs a Jacobi rotation on the rows i and i+1 of the unpacked QR factors.
|
|
============
|
|
*/
|
|
void idMatX::QR_Rotate( idMatX& R, int i, float a, float b )
|
|
{
|
|
int j;
|
|
float f, c, s, w, y;
|
|
|
|
if( a == 0.0f )
|
|
{
|
|
c = 0.0f;
|
|
s = ( b >= 0.0f ) ? 1.0f : -1.0f;
|
|
}
|
|
else if( idMath::Fabs( a ) > idMath::Fabs( b ) )
|
|
{
|
|
f = b / a;
|
|
c = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
|
|
if( a < 0.0f )
|
|
{
|
|
c = -c;
|
|
}
|
|
s = f * c;
|
|
}
|
|
else
|
|
{
|
|
f = a / b;
|
|
s = idMath::Fabs( 1.0f / idMath::Sqrt( 1.0f + f * f ) );
|
|
if( b < 0.0f )
|
|
{
|
|
s = -s;
|
|
}
|
|
c = f * s;
|
|
}
|
|
for( j = i; j < numRows; j++ )
|
|
{
|
|
y = R[i][j];
|
|
w = R[i + 1][j];
|
|
R[i][j] = c * y - s * w;
|
|
R[i + 1][j] = s * y + c * w;
|
|
}
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
y = ( *this )[j][i];
|
|
w = ( *this )[j][i + 1];
|
|
( *this )[j][i] = c * y - s * w;
|
|
( *this )[j][i + 1] = s * y + c * w;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateRankOne
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix: QR + alpha * v * w'
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateRankOne( idMatX& R, const idVecX& v, const idVecX& w, float alpha )
|
|
{
|
|
int i, k;
|
|
float f;
|
|
idVecX u;
|
|
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
|
|
u.SetData( v.GetSize(), VECX_ALLOCA( v.GetSize() ) );
|
|
TransposeMultiply( u, v );
|
|
u *= alpha;
|
|
|
|
for( k = v.GetSize() - 1; k > 0; k-- )
|
|
{
|
|
if( u[k] != 0.0f )
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
for( i = k - 1; i >= 0; i-- )
|
|
{
|
|
QR_Rotate( R, i, u[i], -u[i + 1] );
|
|
if( u[i] == 0.0f )
|
|
{
|
|
u[i] = idMath::Fabs( u[i + 1] );
|
|
}
|
|
else if( idMath::Fabs( u[i] ) > idMath::Fabs( u[i + 1] ) )
|
|
{
|
|
f = u[i + 1] / u[i];
|
|
u[i] = idMath::Fabs( u[i] ) * idMath::Sqrt( 1.0f + f * f );
|
|
}
|
|
else
|
|
{
|
|
f = u[i] / u[i + 1];
|
|
u[i] = idMath::Fabs( u[i + 1] ) * idMath::Sqrt( 1.0f + f * f );
|
|
}
|
|
}
|
|
for( i = 0; i < v.GetSize(); i++ )
|
|
{
|
|
R[0][i] += u[0] * w[i];
|
|
}
|
|
for( i = 0; i < k; i++ )
|
|
{
|
|
QR_Rotate( R, i, -R[i][i], R[i + 1][i] );
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateRowColumn
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
QR + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
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]
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateRowColumn( idMatX& R, const idVecX& v, const idVecX& w, int r )
|
|
{
|
|
idVecX s;
|
|
|
|
assert( v.GetSize() >= numColumns );
|
|
assert( w.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows && r < numColumns );
|
|
assert( w[r] == 0.0f );
|
|
|
|
s.SetData( Max( numRows, numColumns ), VECX_ALLOCA( Max( numRows, numColumns ) ) );
|
|
s.Zero();
|
|
s[r] = 1.0f;
|
|
|
|
if( !QR_UpdateRankOne( R, v, s, 1.0f ) )
|
|
{
|
|
return false;
|
|
}
|
|
if( !QR_UpdateRankOne( R, s, w, 1.0f ) )
|
|
{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateIncrement
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateIncrement( idMatX& R, const idVecX& v, const idVecX& w )
|
|
{
|
|
idVecX v2;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows + 1 );
|
|
assert( w.GetSize() >= numColumns + 1 );
|
|
|
|
ChangeSize( numRows + 1, numColumns + 1, true );
|
|
( *this )[numRows - 1][numRows - 1] = 1.0f;
|
|
|
|
R.ChangeSize( R.numRows + 1, R.numColumns + 1, true );
|
|
R[R.numRows - 1][R.numRows - 1] = 1.0f;
|
|
|
|
v2.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
v2 = v;
|
|
v2[numRows - 1] -= 1.0f;
|
|
|
|
return QR_UpdateRowColumn( R, v2, w, numRows - 1 );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateDecrement
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v and w should store the column and row of the original matrix respectively.
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateDecrement( idMatX& R, const idVecX& v, const idVecX& w, int r )
|
|
{
|
|
idVecX v1, w1;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( w.GetSize() >= numColumns );
|
|
assert( r >= 0 && r < numRows && r < numColumns );
|
|
|
|
v1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
w1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
w1 = -w;
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if( !QR_UpdateRowColumn( R, v1, w1, r ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement( r );
|
|
R.Update_Decrement( r );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: QR
|
|
============
|
|
*/
|
|
void idMatX::QR_Solve( idVecX& x, const idVecX& b, const idVecX& c, const idVecX& d ) const
|
|
{
|
|
int i, j;
|
|
double sum, t;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
|
|
assert( c.GetSize() >= numRows && d.GetSize() >= numRows );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
x[i] = b[i];
|
|
}
|
|
|
|
// multiply b with transpose of Q
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
|
|
sum = 0.0f;
|
|
for( j = i; j < numRows; j++ )
|
|
{
|
|
sum += ( *this )[j][i] * x[j];
|
|
}
|
|
t = sum / c[i];
|
|
for( j = i; j < numRows; j++ )
|
|
{
|
|
x[j] -= t * ( *this )[j][i];
|
|
}
|
|
}
|
|
|
|
// backsubstitution with R
|
|
for( i = numRows - 1; i >= 0; i-- )
|
|
{
|
|
|
|
sum = x[i];
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * x[j];
|
|
}
|
|
x[i] = sum / d[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Solve
|
|
|
|
Solve Ax = b with A factored as: QR
|
|
============
|
|
*/
|
|
void idMatX::QR_Solve( idVecX& x, const idVecX& b, const idMatX& R ) const
|
|
{
|
|
int i, j;
|
|
double sum;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
// multiply b with transpose of Q
|
|
TransposeMultiply( x, b );
|
|
|
|
// backsubstitution with R
|
|
for( i = numRows - 1; i >= 0; i-- )
|
|
{
|
|
|
|
sum = x[i];
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
sum -= R[i][j] * x[j];
|
|
}
|
|
x[i] = sum / R[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: QR
|
|
============
|
|
*/
|
|
void idMatX::QR_Inverse( idMatX& inv, const idVecX& c, const idVecX& d ) const
|
|
{
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
x.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.Zero();
|
|
inv.SetSize( numRows, numColumns );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
b[i] = 1.0f;
|
|
QR_Solve( x, b, c, d );
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UnpackFactors
|
|
|
|
Unpacks the in-place QR factorization.
|
|
============
|
|
*/
|
|
void idMatX::QR_UnpackFactors( idMatX& Q, idMatX& R, const idVecX& c, const idVecX& d ) const
|
|
{
|
|
int i, j, k;
|
|
double sum;
|
|
|
|
Q.Identity( numRows, numColumns );
|
|
for( i = 0; i < numColumns - 1; i++ )
|
|
{
|
|
if( c[i] == 0.0f )
|
|
{
|
|
continue;
|
|
}
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( k = i; k < numColumns; k++ )
|
|
{
|
|
sum += ( *this )[k][i] * Q[j][k];
|
|
}
|
|
sum /= c[i];
|
|
for( k = i; k < numColumns; k++ )
|
|
{
|
|
Q[j][k] -= sum * ( *this )[k][i];
|
|
}
|
|
}
|
|
}
|
|
|
|
R.Zero( numRows, numColumns );
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
R[i][i] = d[i];
|
|
for( j = i + 1; j < numColumns; j++ )
|
|
{
|
|
R[i][j] = ( *this )[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place QR factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::QR_MultiplyFactors( idMatX& m, const idVecX& c, const idVecX& d ) const
|
|
{
|
|
int i, j, k;
|
|
double sum;
|
|
idMatX Q;
|
|
|
|
Q.Identity( numRows, numColumns );
|
|
for( i = 0; i < numColumns - 1; i++ )
|
|
{
|
|
if( c[i] == 0.0f )
|
|
{
|
|
continue;
|
|
}
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( k = i; k < numColumns; k++ )
|
|
{
|
|
sum += ( *this )[k][i] * Q[j][k];
|
|
}
|
|
sum /= c[i];
|
|
for( k = i; k < numColumns; k++ )
|
|
{
|
|
Q[j][k] -= sum * ( *this )[k][i];
|
|
}
|
|
}
|
|
}
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
sum = Q[i][j] * d[i];
|
|
for( k = 0; k < i; k++ )
|
|
{
|
|
sum += Q[i][k] * ( *this )[j][k];
|
|
}
|
|
m[i][j] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Pythag
|
|
|
|
Computes (a^2 + b^2)^1/2 without underflow or overflow.
|
|
============
|
|
*/
|
|
float idMatX::Pythag( float a, float b ) const
|
|
{
|
|
double at, bt, ct;
|
|
|
|
at = idMath::Fabs( a );
|
|
bt = idMath::Fabs( b );
|
|
if( at > bt )
|
|
{
|
|
ct = bt / at;
|
|
return at * idMath::Sqrt( 1.0f + ct * ct );
|
|
}
|
|
else
|
|
{
|
|
if( bt )
|
|
{
|
|
ct = at / bt;
|
|
return bt * idMath::Sqrt( 1.0f + ct * ct );
|
|
}
|
|
else
|
|
{
|
|
return 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_BiDiag
|
|
============
|
|
*/
|
|
void idMatX::SVD_BiDiag( idVecX& w, idVecX& rv1, float& anorm )
|
|
{
|
|
int i, j, k, l;
|
|
double f, h, r, g, s, scale;
|
|
|
|
anorm = 0.0f;
|
|
g = s = scale = 0.0f;
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
l = i + 1;
|
|
rv1[i] = scale * g;
|
|
g = s = scale = 0.0f;
|
|
if( i < numRows )
|
|
{
|
|
for( k = i; k < numRows; k++ )
|
|
{
|
|
scale += idMath::Fabs( ( *this )[k][i] );
|
|
}
|
|
if( scale )
|
|
{
|
|
for( k = i; k < numRows; k++ )
|
|
{
|
|
( *this )[k][i] /= scale;
|
|
s += ( *this )[k][i] * ( *this )[k][i];
|
|
}
|
|
f = ( *this )[i][i];
|
|
g = idMath::Sqrt( s );
|
|
if( f >= 0.0f )
|
|
{
|
|
g = -g;
|
|
}
|
|
h = f * g - s;
|
|
( *this )[i][i] = f - g;
|
|
if( i != ( numColumns - 1 ) )
|
|
{
|
|
for( j = l; j < numColumns; j++ )
|
|
{
|
|
for( s = 0.0f, k = i; k < numRows; k++ )
|
|
{
|
|
s += ( *this )[k][i] * ( *this )[k][j];
|
|
}
|
|
f = s / h;
|
|
for( k = i; k < numRows; k++ )
|
|
{
|
|
( *this )[k][j] += f * ( *this )[k][i];
|
|
}
|
|
}
|
|
}
|
|
for( k = i; k < numRows; k++ )
|
|
{
|
|
( *this )[k][i] *= scale;
|
|
}
|
|
}
|
|
}
|
|
w[i] = scale * g;
|
|
g = s = scale = 0.0f;
|
|
if( i < numRows && i != ( numColumns - 1 ) )
|
|
{
|
|
for( k = l; k < numColumns; k++ )
|
|
{
|
|
scale += idMath::Fabs( ( *this )[i][k] );
|
|
}
|
|
if( scale )
|
|
{
|
|
for( k = l; k < numColumns; k++ )
|
|
{
|
|
( *this )[i][k] /= scale;
|
|
s += ( *this )[i][k] * ( *this )[i][k];
|
|
}
|
|
f = ( *this )[i][l];
|
|
g = idMath::Sqrt( s );
|
|
if( f >= 0.0f )
|
|
{
|
|
g = -g;
|
|
}
|
|
h = 1.0f / ( f * g - s );
|
|
( *this )[i][l] = f - g;
|
|
for( k = l; k < numColumns; k++ )
|
|
{
|
|
rv1[k] = ( *this )[i][k] * h;
|
|
}
|
|
if( i != ( numRows - 1 ) )
|
|
{
|
|
for( j = l; j < numRows; j++ )
|
|
{
|
|
for( s = 0.0f, k = l; k < numColumns; k++ )
|
|
{
|
|
s += ( *this )[j][k] * ( *this )[i][k];
|
|
}
|
|
for( k = l; k < numColumns; k++ )
|
|
{
|
|
( *this )[j][k] += s * rv1[k];
|
|
}
|
|
}
|
|
}
|
|
for( k = l; k < numColumns; k++ )
|
|
{
|
|
( *this )[i][k] *= scale;
|
|
}
|
|
}
|
|
}
|
|
r = idMath::Fabs( w[i] ) + idMath::Fabs( rv1[i] );
|
|
if( r > anorm )
|
|
{
|
|
anorm = r;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_InitialWV
|
|
============
|
|
*/
|
|
void idMatX::SVD_InitialWV( idVecX& w, idMatX& V, idVecX& rv1 )
|
|
{
|
|
int i, j, k, l;
|
|
double f, g, s;
|
|
|
|
g = 0.0f;
|
|
for( i = ( numColumns - 1 ); i >= 0; i-- )
|
|
{
|
|
l = i + 1;
|
|
if( i < ( numColumns - 1 ) )
|
|
{
|
|
if( g )
|
|
{
|
|
for( j = l; j < numColumns; j++ )
|
|
{
|
|
V[j][i] = ( ( *this )[i][j] / ( *this )[i][l] ) / g;
|
|
}
|
|
// double division to reduce underflow
|
|
for( j = l; j < numColumns; j++ )
|
|
{
|
|
for( s = 0.0f, k = l; k < numColumns; k++ )
|
|
{
|
|
s += ( *this )[i][k] * V[k][j];
|
|
}
|
|
for( k = l; k < numColumns; k++ )
|
|
{
|
|
V[k][j] += s * V[k][i];
|
|
}
|
|
}
|
|
}
|
|
for( j = l; j < numColumns; j++ )
|
|
{
|
|
V[i][j] = V[j][i] = 0.0f;
|
|
}
|
|
}
|
|
V[i][i] = 1.0f;
|
|
g = rv1[i];
|
|
}
|
|
for( i = numColumns - 1 ; i >= 0; i-- )
|
|
{
|
|
l = i + 1;
|
|
g = w[i];
|
|
if( i < ( numColumns - 1 ) )
|
|
{
|
|
for( j = l; j < numColumns; j++ )
|
|
{
|
|
( *this )[i][j] = 0.0f;
|
|
}
|
|
}
|
|
if( g )
|
|
{
|
|
g = 1.0f / g;
|
|
if( i != ( numColumns - 1 ) )
|
|
{
|
|
for( j = l; j < numColumns; j++ )
|
|
{
|
|
for( s = 0.0f, k = l; k < numRows; k++ )
|
|
{
|
|
s += ( *this )[k][i] * ( *this )[k][j];
|
|
}
|
|
f = ( s / ( *this )[i][i] ) * g;
|
|
for( k = i; k < numRows; k++ )
|
|
{
|
|
( *this )[k][j] += f * ( *this )[k][i];
|
|
}
|
|
}
|
|
}
|
|
for( j = i; j < numRows; j++ )
|
|
{
|
|
( *this )[j][i] *= g;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for( j = i; j < numRows; j++ )
|
|
{
|
|
( *this )[j][i] = 0.0f;
|
|
}
|
|
}
|
|
( *this )[i][i] += 1.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_Factor
|
|
|
|
in-place factorization: U * Diag(w) * V.Transpose()
|
|
known as the Singular Value Decomposition.
|
|
U is a column-orthogonal matrix which overwrites the original matrix.
|
|
w is a diagonal matrix with all elements >= 0 which are the singular values.
|
|
V is the transpose of an orthogonal matrix.
|
|
============
|
|
*/
|
|
bool idMatX::SVD_Factor( idVecX& w, idMatX& V )
|
|
{
|
|
int flag, i, its, j, jj, k, l, nm;
|
|
double c, f, h, s, x, y, z, r, g = 0.0f;
|
|
float anorm = 0.0f;
|
|
idVecX rv1;
|
|
|
|
if( numRows < numColumns )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
rv1.SetData( numColumns, VECX_ALLOCA( numColumns ) );
|
|
rv1.Zero();
|
|
w.Zero( numColumns );
|
|
V.Zero( numColumns, numColumns );
|
|
|
|
SVD_BiDiag( w, rv1, anorm );
|
|
SVD_InitialWV( w, V, rv1 );
|
|
|
|
for( k = numColumns - 1; k >= 0; k-- )
|
|
{
|
|
for( its = 1; its <= 30; its++ )
|
|
{
|
|
flag = 1;
|
|
nm = 0;
|
|
for( l = k; l >= 0; l-- )
|
|
{
|
|
nm = l - 1;
|
|
if( ( idMath::Fabs( rv1[l] ) + anorm ) == anorm /* idMath::Fabs( rv1[l] ) < idMath::FLT_EPSILON */ )
|
|
{
|
|
flag = 0;
|
|
break;
|
|
}
|
|
if( ( idMath::Fabs( w[nm] ) + anorm ) == anorm /* idMath::Fabs( w[nm] ) < idMath::FLT_EPSILON */ )
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
if( flag )
|
|
{
|
|
c = 0.0f;
|
|
s = 1.0f;
|
|
for( i = l; i <= k; i++ )
|
|
{
|
|
f = s * rv1[i];
|
|
|
|
if( ( idMath::Fabs( f ) + anorm ) != anorm /* idMath::Fabs( f ) > idMath::FLT_EPSILON */ )
|
|
{
|
|
g = w[i];
|
|
h = Pythag( f, g );
|
|
w[i] = h;
|
|
h = 1.0f / h;
|
|
c = g * h;
|
|
s = -f * h;
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
y = ( *this )[j][nm];
|
|
z = ( *this )[j][i];
|
|
( *this )[j][nm] = y * c + z * s;
|
|
( *this )[j][i] = z * c - y * s;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
z = w[k];
|
|
if( l == k )
|
|
{
|
|
if( z < 0.0f )
|
|
{
|
|
w[k] = -z;
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
V[j][k] = -V[j][k];
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
if( its == 30 )
|
|
{
|
|
return false; // no convergence
|
|
}
|
|
x = w[l];
|
|
nm = k - 1;
|
|
y = w[nm];
|
|
g = rv1[nm];
|
|
h = rv1[k];
|
|
f = ( ( y - z ) * ( y + z ) + ( g - h ) * ( g + h ) ) / ( 2.0f * h * y );
|
|
g = Pythag( f, 1.0f );
|
|
r = ( f >= 0.0f ? g : - g );
|
|
f = ( ( x - z ) * ( x + z ) + h * ( ( y / ( f + r ) ) - h ) ) / x;
|
|
c = s = 1.0f;
|
|
for( j = l; j <= nm; j++ )
|
|
{
|
|
i = j + 1;
|
|
g = rv1[i];
|
|
y = w[i];
|
|
h = s * g;
|
|
g = c * g;
|
|
z = Pythag( f, h );
|
|
rv1[j] = z;
|
|
c = f / z;
|
|
s = h / z;
|
|
f = x * c + g * s;
|
|
g = g * c - x * s;
|
|
h = y * s;
|
|
y = y * c;
|
|
for( jj = 0; jj < numColumns; jj++ )
|
|
{
|
|
x = V[jj][j];
|
|
z = V[jj][i];
|
|
V[jj][j] = x * c + z * s;
|
|
V[jj][i] = z * c - x * s;
|
|
}
|
|
z = Pythag( f, h );
|
|
w[j] = z;
|
|
if( z )
|
|
{
|
|
z = 1.0f / z;
|
|
c = f * z;
|
|
s = h * z;
|
|
}
|
|
f = ( c * g ) + ( s * y );
|
|
x = ( c * y ) - ( s * g );
|
|
for( jj = 0; jj < numRows; jj++ )
|
|
{
|
|
y = ( *this )[jj][j];
|
|
z = ( *this )[jj][i];
|
|
( *this )[jj][j] = y * c + z * s;
|
|
( *this )[jj][i] = z * c - y * s;
|
|
}
|
|
}
|
|
rv1[l] = 0.0f;
|
|
rv1[k] = f;
|
|
w[k] = x;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_Solve
|
|
|
|
Solve Ax = b with A factored as: U * Diag(w) * V.Transpose()
|
|
============
|
|
*/
|
|
void idMatX::SVD_Solve( idVecX& x, const idVecX& b, const idVecX& w, const idMatX& V ) const
|
|
{
|
|
int i, j;
|
|
double sum;
|
|
idVecX tmp;
|
|
|
|
assert( x.GetSize() >= numColumns );
|
|
assert( b.GetSize() >= numColumns );
|
|
assert( w.GetSize() == numColumns );
|
|
assert( V.GetNumRows() == numColumns && V.GetNumColumns() == numColumns );
|
|
|
|
tmp.SetData( numColumns, VECX_ALLOCA( numColumns ) );
|
|
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
sum = 0.0f;
|
|
if( w[i] >= idMath::FLT_EPSILON )
|
|
{
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
sum += ( *this )[j][i] * b[j];
|
|
}
|
|
sum /= w[i];
|
|
}
|
|
tmp[i] = sum;
|
|
}
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
sum += V[i][j] * tmp[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: U * Diag(w) * V.Transpose()
|
|
============
|
|
*/
|
|
void idMatX::SVD_Inverse( idMatX& inv, const idVecX& w, const idMatX& V ) const
|
|
{
|
|
int i, j, k;
|
|
double wi, sum;
|
|
idMatX V2;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
V2 = V;
|
|
|
|
// V * [diag(1/w[i])]
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
wi = w[i];
|
|
wi = ( wi < idMath::FLT_EPSILON ) ? 0.0f : 1.0f / wi;
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
V2[j][i] *= wi;
|
|
}
|
|
}
|
|
|
|
// V * [diag(1/w[i])] * Ut
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
sum = V2[i][0] * ( *this )[j][0];
|
|
for( k = 1; k < numColumns; k++ )
|
|
{
|
|
sum += V2[i][k] * ( *this )[j][k];
|
|
}
|
|
inv[i][j] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place SVD factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::SVD_MultiplyFactors( idMatX& m, const idVecX& w, const idMatX& V ) const
|
|
{
|
|
int r, i, j;
|
|
double sum;
|
|
|
|
m.SetSize( numRows, V.GetNumRows() );
|
|
|
|
for( r = 0; r < numRows; r++ )
|
|
{
|
|
// calculate row of matrix
|
|
if( w[r] >= idMath::FLT_EPSILON )
|
|
{
|
|
for( i = 0; i < V.GetNumRows(); i++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( j = 0; j < numColumns; j++ )
|
|
{
|
|
sum += ( *this )[r][j] * V[i][j];
|
|
}
|
|
m[r][i] = sum * w[r];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
for( i = 0; i < V.GetNumRows(); i++ )
|
|
{
|
|
m[r][i] = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_Factor
|
|
|
|
in-place Cholesky factorization: LL'
|
|
L is a triangular matrix stored in the lower triangle.
|
|
The upper triangle is not cleared.
|
|
The initial matrix has to be symmetric positive definite.
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_Factor()
|
|
{
|
|
int i, j, k;
|
|
float* invSqrt;
|
|
double sum;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
invSqrt = ( float* ) _alloca16( numRows * sizeof( float ) );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
|
|
sum = ( *this )[i][j];
|
|
for( k = 0; k < j; k++ )
|
|
{
|
|
sum -= ( *this )[i][k] * ( *this )[j][k];
|
|
}
|
|
( *this )[i][j] = sum * invSqrt[j];
|
|
}
|
|
|
|
sum = ( *this )[i][i];
|
|
for( k = 0; k < i; k++ )
|
|
{
|
|
sum -= ( *this )[i][k] * ( *this )[i][k];
|
|
}
|
|
|
|
if( sum <= 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
invSqrt[i] = idMath::InvSqrt( sum );
|
|
( *this )[i][i] = invSqrt[i] * sum;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateRankOne
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix: LL' + alpha * v * v'
|
|
If offset > 0 only the lower right corner starting at (offset, offset) is updated.
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateRankOne( const idVecX& v, float alpha, int offset )
|
|
{
|
|
int i, j;
|
|
float* y;
|
|
double diag, invDiag, diagSqr, newDiag, newDiagSqr, beta, p, d;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( offset >= 0 && offset < numRows );
|
|
|
|
y = ( float* ) _alloca16( v.GetSize() * sizeof( float ) );
|
|
memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
|
|
|
|
for( i = offset; i < numColumns; i++ )
|
|
{
|
|
p = y[i];
|
|
diag = ( *this )[i][i];
|
|
invDiag = 1.0f / diag;
|
|
diagSqr = diag * diag;
|
|
newDiagSqr = diagSqr + alpha * p * p;
|
|
|
|
if( newDiagSqr <= 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
( *this )[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
|
|
|
|
alpha /= newDiagSqr;
|
|
beta = p * alpha;
|
|
alpha *= diagSqr;
|
|
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
|
|
d = ( *this )[j][i] * invDiag;
|
|
|
|
y[j] -= p * d;
|
|
d += beta * y[j];
|
|
|
|
( *this )[j][i] = d * newDiag;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateRowColumn
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
LL' + [ a b c ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateRowColumn( const idVecX& v, int r )
|
|
{
|
|
int i, j;
|
|
double sum;
|
|
float* original, *y;
|
|
idVecX addSub;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows );
|
|
|
|
addSub.SetData( numColumns, ( float* ) _alloca16( numColumns * sizeof( float ) ) );
|
|
|
|
if( r == 0 )
|
|
{
|
|
|
|
if( numColumns == 1 )
|
|
{
|
|
double v0 = v[0];
|
|
sum = ( *this )[0][0];
|
|
sum = sum * sum;
|
|
sum = sum + v0;
|
|
if( sum <= 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
( *this )[0][0] = idMath::Sqrt( sum );
|
|
return true;
|
|
}
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
addSub[i] = v[i];
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
original = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
y = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
|
|
// calculate original row/column of matrix
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( j = 0; j <= i; j++ )
|
|
{
|
|
sum += ( *this )[r][j] * ( *this )[i][j];
|
|
}
|
|
original[i] = sum;
|
|
}
|
|
|
|
// solve for y in L * y = original + v
|
|
for( i = 0; i < r; i++ )
|
|
{
|
|
sum = original[i] + v[i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[r][j] * ( *this )[i][j];
|
|
}
|
|
( *this )[r][i] = sum / ( *this )[i][i];
|
|
}
|
|
|
|
// if the last row/column of the matrix is updated
|
|
if( r == numColumns - 1 )
|
|
{
|
|
// only calculate new diagonal
|
|
sum = original[r] + v[r];
|
|
for( j = 0; j < r; j++ )
|
|
{
|
|
sum -= ( *this )[r][j] * ( *this )[r][j];
|
|
}
|
|
if( sum <= 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
( *this )[r][r] = idMath::Sqrt( sum );
|
|
return true;
|
|
}
|
|
|
|
// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
|
|
for( i = r; i < numColumns; i++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( j = 0; j <= r; j++ )
|
|
{
|
|
sum += ( *this )[r][j] * ( *this )[i][j];
|
|
}
|
|
addSub[i] = v[i] - ( sum - original[i] );
|
|
}
|
|
}
|
|
|
|
// add row/column to the lower right sub matrix starting at (r, r)
|
|
|
|
#if 0
|
|
|
|
idVecX v1, v2;
|
|
double d;
|
|
|
|
v1.SetData( numColumns, ( float* ) _alloca16( numColumns * sizeof( float ) ) );
|
|
v2.SetData( numColumns, ( float* ) _alloca16( numColumns * sizeof( float ) ) );
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
|
|
v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
|
|
for( i = r + 1; i < numColumns; i++ )
|
|
{
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
// update
|
|
if( !Cholesky_UpdateRankOne( v1, 1.0f, r ) )
|
|
{
|
|
return false;
|
|
}
|
|
// downdate
|
|
if( !Cholesky_UpdateRankOne( v2, -1.0f, r ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
#else
|
|
|
|
float* v1, *v2;
|
|
double diag, invDiag, diagSqr, newDiag, newDiagSqr;
|
|
double alpha1, alpha2, beta1, beta2, p1, p2, d;
|
|
|
|
v1 = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
v2 = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
|
|
v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
|
|
for( i = r + 1; i < numColumns; i++ )
|
|
{
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
alpha1 = 1.0f;
|
|
alpha2 = -1.0f;
|
|
|
|
// simultaneous update/downdate of the sub matrix starting at (r, r)
|
|
for( i = r; i < numColumns; i++ )
|
|
{
|
|
p1 = v1[i];
|
|
diag = ( *this )[i][i];
|
|
invDiag = 1.0f / diag;
|
|
diagSqr = diag * diag;
|
|
newDiagSqr = diagSqr + alpha1 * p1 * p1;
|
|
|
|
if( newDiagSqr <= 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
alpha1 /= newDiagSqr;
|
|
beta1 = p1 * alpha1;
|
|
alpha1 *= diagSqr;
|
|
|
|
p2 = v2[i];
|
|
diagSqr = newDiagSqr;
|
|
newDiagSqr = diagSqr + alpha2 * p2 * p2;
|
|
|
|
if( newDiagSqr <= 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
( *this )[i][i] = newDiag = idMath::Sqrt( newDiagSqr );
|
|
|
|
alpha2 /= newDiagSqr;
|
|
beta2 = p2 * alpha2;
|
|
alpha2 *= diagSqr;
|
|
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
|
|
d = ( *this )[j][i] * invDiag;
|
|
|
|
v1[j] -= p1 * d;
|
|
d += beta1 * v1[j];
|
|
|
|
v2[j] -= p2 * d;
|
|
d += beta2 * v2[j];
|
|
|
|
( *this )[j][i] = d * newDiag;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateIncrement
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ a b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows]
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateIncrement( const idVecX& v )
|
|
{
|
|
int i, j;
|
|
float* x;
|
|
double sum;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows + 1 );
|
|
|
|
ChangeSize( numRows + 1, numColumns + 1, false );
|
|
|
|
x = ( float* ) _alloca16( numRows * sizeof( float ) );
|
|
|
|
// solve for x in L * x = v
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
sum = v[i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * x[j];
|
|
}
|
|
x[i] = sum / ( *this )[i][i];
|
|
}
|
|
|
|
// calculate new row of L and calculate the square of the diagonal entry
|
|
sum = v[numRows - 1];
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
( *this )[numRows - 1][i] = x[i];
|
|
sum -= x[i] * x[i];
|
|
}
|
|
|
|
if( sum <= 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// store the diagonal entry
|
|
( *this )[numRows - 1][numRows - 1] = idMath::Sqrt( sum );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateDecrement
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v should store the row of the original matrix.
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateDecrement( const idVecX& v, int r )
|
|
{
|
|
idVecX v1;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows );
|
|
|
|
v1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
v1[r] += 1.0f;
|
|
|
|
// NOTE: msvc compiler bug: the this pointer stored in edi is expected to stay
|
|
// untouched when calling Cholesky_UpdateRowColumn in the if statement
|
|
#if 0
|
|
if( !Cholesky_UpdateRowColumn( v1, r ) )
|
|
{
|
|
#else
|
|
bool ret = Cholesky_UpdateRowColumn( v1, r );
|
|
if( !ret )
|
|
{
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement( r );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: LL'
|
|
============
|
|
*/
|
|
void idMatX::Cholesky_Solve( idVecX& x, const idVecX& b ) const
|
|
{
|
|
int i, j;
|
|
double sum;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
|
|
|
|
// solve L
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
sum = b[i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * x[j];
|
|
}
|
|
x[i] = sum / ( *this )[i][i];
|
|
}
|
|
|
|
// solve Lt
|
|
for( i = numRows - 1; i >= 0; i-- )
|
|
{
|
|
sum = x[i];
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
sum -= ( *this )[j][i] * x[j];
|
|
}
|
|
x[i] = sum / ( *this )[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: LL'
|
|
============
|
|
*/
|
|
void idMatX::Cholesky_Inverse( idMatX& inv ) const
|
|
{
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
x.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.Zero();
|
|
inv.SetSize( numRows, numColumns );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
b[i] = 1.0f;
|
|
Cholesky_Solve( x, b );
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place Cholesky factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::Cholesky_MultiplyFactors( idMatX& m ) const
|
|
{
|
|
int r, i, j;
|
|
double sum;
|
|
|
|
m.SetSize( numRows, numColumns );
|
|
|
|
for( r = 0; r < numRows; r++ )
|
|
{
|
|
|
|
// calculate row of matrix
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( j = 0; j <= i && j <= r; j++ )
|
|
{
|
|
sum += ( *this )[r][j] * ( *this )[i][j];
|
|
}
|
|
m[r][i] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_Factor
|
|
|
|
in-place factorization: LDL'
|
|
L is a triangular matrix stored in the lower triangle.
|
|
L has ones on the diagonal that are not stored.
|
|
D is a diagonal matrix stored on the diagonal.
|
|
The upper triangle is not cleared.
|
|
The initial matrix has to be symmetric.
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_Factor()
|
|
{
|
|
int i, j, k;
|
|
float* v;
|
|
double d, sum;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
v = ( float* ) _alloca16( numRows * sizeof( float ) );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
sum = ( *this )[i][i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
d = ( *this )[i][j];
|
|
v[j] = ( *this )[j][j] * d;
|
|
sum -= v[j] * d;
|
|
}
|
|
|
|
if( sum == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
( *this )[i][i] = sum;
|
|
d = 1.0f / sum;
|
|
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
sum = ( *this )[j][i];
|
|
for( k = 0; k < i; k++ )
|
|
{
|
|
sum -= ( *this )[j][k] * v[k];
|
|
}
|
|
( *this )[j][i] = sum * d;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateRankOne
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix: LDL' + alpha * v * v'
|
|
If offset > 0 only the lower right corner starting at (offset, offset) is updated.
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateRankOne( const idVecX& v, float alpha, int offset )
|
|
{
|
|
int i, j;
|
|
float* y;
|
|
double diag, newDiag, beta, p, d;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( offset >= 0 && offset < numRows );
|
|
|
|
y = ( float* ) _alloca16( v.GetSize() * sizeof( float ) );
|
|
memcpy( y, v.ToFloatPtr(), v.GetSize() * sizeof( float ) );
|
|
|
|
for( i = offset; i < numColumns; i++ )
|
|
{
|
|
p = y[i];
|
|
diag = ( *this )[i][i];
|
|
( *this )[i][i] = newDiag = diag + alpha * p * p;
|
|
|
|
if( newDiag == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
alpha /= newDiag;
|
|
beta = p * alpha;
|
|
alpha *= diag;
|
|
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
|
|
d = ( *this )[j][i];
|
|
|
|
y[j] -= p * d;
|
|
d += beta * y[j];
|
|
|
|
( *this )[j][i] = d;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateRowColumn
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
LDL' + [ a b c ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateRowColumn( const idVecX& v, int r )
|
|
{
|
|
int i, j;
|
|
double sum;
|
|
float* original, *y;
|
|
idVecX addSub;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows );
|
|
|
|
addSub.SetData( numColumns, ( float* ) _alloca16( numColumns * sizeof( float ) ) );
|
|
|
|
if( r == 0 )
|
|
{
|
|
|
|
if( numColumns == 1 )
|
|
{
|
|
( *this )[0][0] += v[0];
|
|
return true;
|
|
}
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
addSub[i] = v[i];
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
|
|
original = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
y = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
|
|
// calculate original row/column of matrix
|
|
for( i = 0; i < r; i++ )
|
|
{
|
|
y[i] = ( *this )[r][i] * ( *this )[i][i];
|
|
}
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
if( i < r )
|
|
{
|
|
sum = ( *this )[i][i] * ( *this )[r][i];
|
|
}
|
|
else if( i == r )
|
|
{
|
|
sum = ( *this )[r][r];
|
|
}
|
|
else
|
|
{
|
|
sum = ( *this )[r][r] * ( *this )[i][r];
|
|
}
|
|
for( j = 0; j < i && j < r; j++ )
|
|
{
|
|
sum += ( *this )[i][j] * y[j];
|
|
}
|
|
original[i] = sum;
|
|
}
|
|
|
|
// solve for y in L * y = original + v
|
|
for( i = 0; i < r; i++ )
|
|
{
|
|
sum = original[i] + v[i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * y[j];
|
|
}
|
|
y[i] = sum;
|
|
}
|
|
|
|
// calculate new row of L
|
|
for( i = 0; i < r; i++ )
|
|
{
|
|
( *this )[r][i] = y[i] / ( *this )[i][i];
|
|
}
|
|
|
|
// if the last row/column of the matrix is updated
|
|
if( r == numColumns - 1 )
|
|
{
|
|
// only calculate new diagonal
|
|
sum = original[r] + v[r];
|
|
for( j = 0; j < r; j++ )
|
|
{
|
|
sum -= ( *this )[r][j] * y[j];
|
|
}
|
|
if( sum == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
( *this )[r][r] = sum;
|
|
return true;
|
|
}
|
|
|
|
// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
|
|
for( i = 0; i < r; i++ )
|
|
{
|
|
y[i] = ( *this )[r][i] * ( *this )[i][i];
|
|
}
|
|
for( i = r; i < numColumns; i++ )
|
|
{
|
|
if( i == r )
|
|
{
|
|
sum = ( *this )[r][r];
|
|
}
|
|
else
|
|
{
|
|
sum = ( *this )[r][r] * ( *this )[i][r];
|
|
}
|
|
for( j = 0; j < r; j++ )
|
|
{
|
|
sum += ( *this )[i][j] * y[j];
|
|
}
|
|
addSub[i] = v[i] - ( sum - original[i] );
|
|
}
|
|
}
|
|
|
|
// add row/column to the lower right sub matrix starting at (r, r)
|
|
|
|
#if 0
|
|
|
|
idVecX v1, v2;
|
|
double d;
|
|
|
|
v1.SetData( numColumns, ( float* ) _alloca16( numColumns * sizeof( float ) ) );
|
|
v2.SetData( numColumns, ( float* ) _alloca16( numColumns * sizeof( float ) ) );
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
|
|
v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
|
|
for( i = r + 1; i < numColumns; i++ )
|
|
{
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
// update
|
|
if( !LDLT_UpdateRankOne( v1, 1.0f, r ) )
|
|
{
|
|
return false;
|
|
}
|
|
// downdate
|
|
if( !LDLT_UpdateRankOne( v2, -1.0f, r ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
#else
|
|
|
|
float* v1, *v2;
|
|
double d, diag, newDiag, p1, p2, alpha1, alpha2, beta1, beta2;
|
|
|
|
v1 = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
v2 = ( float* ) _alloca16( numColumns * sizeof( float ) );
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = ( 0.5f * addSub[r] + 1.0f ) * d;
|
|
v2[r] = ( 0.5f * addSub[r] - 1.0f ) * d;
|
|
for( i = r + 1; i < numColumns; i++ )
|
|
{
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
alpha1 = 1.0f;
|
|
alpha2 = -1.0f;
|
|
|
|
// simultaneous update/downdate of the sub matrix starting at (r, r)
|
|
for( i = r; i < numColumns; i++ )
|
|
{
|
|
|
|
diag = ( *this )[i][i];
|
|
p1 = v1[i];
|
|
newDiag = diag + alpha1 * p1 * p1;
|
|
|
|
if( newDiag == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
alpha1 /= newDiag;
|
|
beta1 = p1 * alpha1;
|
|
alpha1 *= diag;
|
|
|
|
diag = newDiag;
|
|
p2 = v2[i];
|
|
newDiag = diag + alpha2 * p2 * p2;
|
|
|
|
if( newDiag == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
alpha2 /= newDiag;
|
|
beta2 = p2 * alpha2;
|
|
alpha2 *= diag;
|
|
|
|
( *this )[i][i] = newDiag;
|
|
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
|
|
d = ( *this )[j][i];
|
|
|
|
v1[j] -= p1 * d;
|
|
d += beta1 * v1[j];
|
|
|
|
v2[j] -= p2 * d;
|
|
d += beta2 * v2[j];
|
|
|
|
( *this )[j][i] = d;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateIncrement
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ a b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows]
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateIncrement( const idVecX& v )
|
|
{
|
|
int i, j;
|
|
float* x;
|
|
double sum, d;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows + 1 );
|
|
|
|
ChangeSize( numRows + 1, numColumns + 1, false );
|
|
|
|
x = ( float* ) _alloca16( numRows * sizeof( float ) );
|
|
|
|
// solve for x in L * x = v
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
sum = v[i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
|
|
// calculate new row of L and calculate the diagonal entry
|
|
sum = v[numRows - 1];
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
( *this )[numRows - 1][i] = d = x[i] / ( *this )[i][i];
|
|
sum -= d * x[i];
|
|
}
|
|
|
|
if( sum == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// store the diagonal entry
|
|
( *this )[numRows - 1][numRows - 1] = sum;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateDecrement
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v should store the row of the original matrix.
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateDecrement( const idVecX& v, int r )
|
|
{
|
|
idVecX v1;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( v.GetSize() >= numRows );
|
|
assert( r >= 0 && r < numRows );
|
|
|
|
v1.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
v1[r] += 1.0f;
|
|
|
|
// NOTE: msvc compiler bug: the this pointer stored in edi is expected to stay
|
|
// untouched when calling LDLT_UpdateRowColumn in the if statement
|
|
#if 0
|
|
if( !LDLT_UpdateRowColumn( v1, r ) )
|
|
{
|
|
#else
|
|
bool ret = LDLT_UpdateRowColumn( v1, r );
|
|
if( !ret )
|
|
{
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement( r );
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: LDL'
|
|
============
|
|
*/
|
|
void idMatX::LDLT_Solve( idVecX& x, const idVecX& b ) const
|
|
{
|
|
int i, j;
|
|
double sum;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
|
|
|
|
// solve L
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
sum = b[i];
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
sum -= ( *this )[i][j] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
|
|
// solve D
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
x[i] /= ( *this )[i][i];
|
|
}
|
|
|
|
// solve Lt
|
|
for( i = numRows - 2; i >= 0; i-- )
|
|
{
|
|
sum = x[i];
|
|
for( j = i + 1; j < numRows; j++ )
|
|
{
|
|
sum -= ( *this )[j][i] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: LDL'
|
|
============
|
|
*/
|
|
void idMatX::LDLT_Inverse( idMatX& inv ) const
|
|
{
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
x.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.Zero();
|
|
inv.SetSize( numRows, numColumns );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
b[i] = 1.0f;
|
|
LDLT_Solve( x, b );
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UnpackFactors
|
|
|
|
Unpacks the in-place LDL' factorization.
|
|
============
|
|
*/
|
|
void idMatX::LDLT_UnpackFactors( idMatX& L, idMatX& D ) const
|
|
{
|
|
int i, j;
|
|
|
|
L.Zero( numRows, numColumns );
|
|
D.Zero( numRows, numColumns );
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
for( j = 0; j < i; j++ )
|
|
{
|
|
L[i][j] = ( *this )[i][j];
|
|
}
|
|
L[i][i] = 1.0f;
|
|
D[i][i] = ( *this )[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place LDL' factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::LDLT_MultiplyFactors( idMatX& m ) const
|
|
{
|
|
int r, i, j;
|
|
float* v;
|
|
double sum;
|
|
|
|
v = ( float* ) _alloca16( numRows * sizeof( float ) );
|
|
m.SetSize( numRows, numColumns );
|
|
|
|
for( r = 0; r < numRows; r++ )
|
|
{
|
|
|
|
// calculate row of matrix
|
|
for( i = 0; i < r; i++ )
|
|
{
|
|
v[i] = ( *this )[r][i] * ( *this )[i][i];
|
|
}
|
|
for( i = 0; i < numColumns; i++ )
|
|
{
|
|
if( i < r )
|
|
{
|
|
sum = ( *this )[i][i] * ( *this )[r][i];
|
|
}
|
|
else if( i == r )
|
|
{
|
|
sum = ( *this )[r][r];
|
|
}
|
|
else
|
|
{
|
|
sum = ( *this )[r][r] * ( *this )[i][r];
|
|
}
|
|
for( j = 0; j < i && j < r; j++ )
|
|
{
|
|
sum += ( *this )[i][j] * v[j];
|
|
}
|
|
m[r][i] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::TriDiagonal_ClearTriangles
|
|
============
|
|
*/
|
|
void idMatX::TriDiagonal_ClearTriangles()
|
|
{
|
|
int i, j;
|
|
|
|
assert( numRows == numColumns );
|
|
for( i = 0; i < numRows - 2; i++ )
|
|
{
|
|
for( j = i + 2; j < numColumns; j++ )
|
|
{
|
|
( *this )[i][j] = 0.0f;
|
|
( *this )[j][i] = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::TriDiagonal_Solve
|
|
|
|
Solve Ax = b with A being tridiagonal.
|
|
============
|
|
*/
|
|
bool idMatX::TriDiagonal_Solve( idVecX& x, const idVecX& b ) const
|
|
{
|
|
int i;
|
|
float d;
|
|
idVecX tmp;
|
|
|
|
assert( numRows == numColumns );
|
|
assert( x.GetSize() >= numRows && b.GetSize() >= numRows );
|
|
|
|
tmp.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
d = ( *this )[0][0];
|
|
if( d == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
d = 1.0f / d;
|
|
x[0] = b[0] * d;
|
|
for( i = 1; i < numRows; i++ )
|
|
{
|
|
tmp[i] = ( *this )[i - 1][i] * d;
|
|
d = ( *this )[i][i] - ( *this )[i][i - 1] * tmp[i];
|
|
if( d == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
d = 1.0f / d;
|
|
x[i] = ( b[i] - ( *this )[i][i - 1] * x[i - 1] ) * d;
|
|
}
|
|
for( i = numRows - 2; i >= 0; i-- )
|
|
{
|
|
x[i] -= tmp[i + 1] * x[i + 1];
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::TriDiagonal_Inverse
|
|
|
|
Calculates the inverse of a tri-diagonal matrix.
|
|
============
|
|
*/
|
|
void idMatX::TriDiagonal_Inverse( idMatX& inv ) const
|
|
{
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
x.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.Zero();
|
|
inv.SetSize( numRows, numColumns );
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
b[i] = 1.0f;
|
|
TriDiagonal_Solve( x, b );
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::HouseholderReduction
|
|
|
|
Householder reduction to symmetric tri-diagonal form.
|
|
The original matrix is replaced by an orthogonal matrix effecting the accumulated householder transformations.
|
|
The diagonal elements of the diagonal matrix are stored in diag.
|
|
The off-diagonal elements of the diagonal matrix are stored in subd.
|
|
The initial matrix has to be symmetric.
|
|
============
|
|
*/
|
|
void idMatX::HouseholderReduction( idVecX& diag, idVecX& subd )
|
|
{
|
|
int i0, i1, i2, i3;
|
|
float h, f, g, invH, halfFdivH, scale, invScale, sum;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
diag.SetSize( numRows );
|
|
subd.SetSize( numRows );
|
|
|
|
for( i0 = numRows - 1, i3 = numRows - 2; i0 >= 1; i0--, i3-- )
|
|
{
|
|
h = 0.0f;
|
|
scale = 0.0f;
|
|
|
|
if( i3 > 0 )
|
|
{
|
|
for( i2 = 0; i2 <= i3; i2++ )
|
|
{
|
|
scale += idMath::Fabs( ( *this )[i0][i2] );
|
|
}
|
|
if( scale == 0 )
|
|
{
|
|
subd[i0] = ( *this )[i0][i3];
|
|
}
|
|
else
|
|
{
|
|
invScale = 1.0f / scale;
|
|
for( i2 = 0; i2 <= i3; i2++ )
|
|
{
|
|
( *this )[i0][i2] *= invScale;
|
|
h += ( *this )[i0][i2] * ( *this )[i0][i2];
|
|
}
|
|
f = ( *this )[i0][i3];
|
|
g = idMath::Sqrt( h );
|
|
if( f > 0.0f )
|
|
{
|
|
g = -g;
|
|
}
|
|
subd[i0] = scale * g;
|
|
h -= f * g;
|
|
( *this )[i0][i3] = f - g;
|
|
f = 0.0f;
|
|
invH = 1.0f / h;
|
|
for( i1 = 0; i1 <= i3; i1++ )
|
|
{
|
|
( *this )[i1][i0] = ( *this )[i0][i1] * invH;
|
|
g = 0.0f;
|
|
for( i2 = 0; i2 <= i1; i2++ )
|
|
{
|
|
g += ( *this )[i1][i2] * ( *this )[i0][i2];
|
|
}
|
|
for( i2 = i1 + 1; i2 <= i3; i2++ )
|
|
{
|
|
g += ( *this )[i2][i1] * ( *this )[i0][i2];
|
|
}
|
|
subd[i1] = g * invH;
|
|
f += subd[i1] * ( *this )[i0][i1];
|
|
}
|
|
halfFdivH = 0.5f * f * invH;
|
|
for( i1 = 0; i1 <= i3; i1++ )
|
|
{
|
|
f = ( *this )[i0][i1];
|
|
g = subd[i1] - halfFdivH * f;
|
|
subd[i1] = g;
|
|
for( i2 = 0; i2 <= i1; i2++ )
|
|
{
|
|
( *this )[i1][i2] -= f * subd[i2] + g * ( *this )[i0][i2];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
subd[i0] = ( *this )[i0][i3];
|
|
}
|
|
|
|
diag[i0] = h;
|
|
}
|
|
|
|
diag[0] = 0.0f;
|
|
subd[0] = 0.0f;
|
|
for( i0 = 0, i3 = -1; i0 <= numRows - 1; i0++, i3++ )
|
|
{
|
|
if( diag[i0] )
|
|
{
|
|
for( i1 = 0; i1 <= i3; i1++ )
|
|
{
|
|
sum = 0.0f;
|
|
for( i2 = 0; i2 <= i3; i2++ )
|
|
{
|
|
sum += ( *this )[i0][i2] * ( *this )[i2][i1];
|
|
}
|
|
for( i2 = 0; i2 <= i3; i2++ )
|
|
{
|
|
( *this )[i2][i1] -= sum * ( *this )[i2][i0];
|
|
}
|
|
}
|
|
}
|
|
diag[i0] = ( *this )[i0][i0];
|
|
( *this )[i0][i0] = 1.0f;
|
|
for( i1 = 0; i1 <= i3; i1++ )
|
|
{
|
|
( *this )[i1][i0] = 0.0f;
|
|
( *this )[i0][i1] = 0.0f;
|
|
}
|
|
}
|
|
|
|
// re-order
|
|
for( i0 = 1, i3 = 0; i0 < numRows; i0++, i3++ )
|
|
{
|
|
subd[i3] = subd[i0];
|
|
}
|
|
subd[numRows - 1] = 0.0f;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QL
|
|
|
|
QL algorithm with implicit shifts to determine the eigenvalues and eigenvectors of a symmetric tri-diagonal matrix.
|
|
diag contains the diagonal elements of the symmetric tri-diagonal matrix on input and is overwritten with the eigenvalues.
|
|
subd contains the off-diagonal elements of the symmetric tri-diagonal matrix and is destroyed.
|
|
This matrix has to be either the identity matrix to determine the eigenvectors for a symmetric tri-diagonal matrix,
|
|
or the matrix returned by the Householder reduction to determine the eigenvalues for the original symmetric matrix.
|
|
============
|
|
*/
|
|
bool idMatX::QL( idVecX& diag, idVecX& subd )
|
|
{
|
|
const int maxIter = 32;
|
|
int i0, i1, i2, i3;
|
|
float a, b, f, g, r, p, s, c;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
for( i0 = 0; i0 < numRows; i0++ )
|
|
{
|
|
for( i1 = 0; i1 < maxIter; i1++ )
|
|
{
|
|
for( i2 = i0; i2 <= numRows - 2; i2++ )
|
|
{
|
|
a = idMath::Fabs( diag[i2] ) + idMath::Fabs( diag[i2 + 1] );
|
|
if( idMath::Fabs( subd[i2] ) + a == a )
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
if( i2 == i0 )
|
|
{
|
|
break;
|
|
}
|
|
|
|
g = ( diag[i0 + 1] - diag[i0] ) / ( 2.0f * subd[i0] );
|
|
r = idMath::Sqrt( g * g + 1.0f );
|
|
if( g < 0.0f )
|
|
{
|
|
g = diag[i2] - diag[i0] + subd[i0] / ( g - r );
|
|
}
|
|
else
|
|
{
|
|
g = diag[i2] - diag[i0] + subd[i0] / ( g + r );
|
|
}
|
|
s = 1.0f;
|
|
c = 1.0f;
|
|
p = 0.0f;
|
|
for( i3 = i2 - 1; i3 >= i0; i3-- )
|
|
{
|
|
f = s * subd[i3];
|
|
b = c * subd[i3];
|
|
if( idMath::Fabs( f ) >= idMath::Fabs( g ) )
|
|
{
|
|
c = g / f;
|
|
r = idMath::Sqrt( c * c + 1.0f );
|
|
subd[i3 + 1] = f * r;
|
|
s = 1.0f / r;
|
|
c *= s;
|
|
}
|
|
else
|
|
{
|
|
s = f / g;
|
|
r = idMath::Sqrt( s * s + 1.0f );
|
|
subd[i3 + 1] = g * r;
|
|
c = 1.0f / r;
|
|
s *= c;
|
|
}
|
|
g = diag[i3 + 1] - p;
|
|
r = ( diag[i3] - g ) * s + 2.0f * b * c;
|
|
p = s * r;
|
|
diag[i3 + 1] = g + p;
|
|
g = c * r - b;
|
|
|
|
for( int i4 = 0; i4 < numRows; i4++ )
|
|
{
|
|
f = ( *this )[i4][i3 + 1];
|
|
( *this )[i4][i3 + 1] = s * ( *this )[i4][i3] + c * f;
|
|
( *this )[i4][i3] = c * ( *this )[i4][i3] - s * f;
|
|
}
|
|
}
|
|
diag[i0] -= p;
|
|
subd[i0] = g;
|
|
subd[i2] = 0.0f;
|
|
}
|
|
if( i1 == maxIter )
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SolveSymmetricTriDiagonal
|
|
|
|
Determine eigen values and eigen vectors for a symmetric tri-diagonal matrix.
|
|
The eigen values are stored in 'eigenValues'.
|
|
Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
|
|
The initial matrix has to be symmetric tri-diagonal.
|
|
============
|
|
*/
|
|
bool idMatX::Eigen_SolveSymmetricTriDiagonal( idVecX& eigenValues )
|
|
{
|
|
int i;
|
|
idVecX subd;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
subd.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
eigenValues.SetSize( numRows );
|
|
|
|
for( i = 0; i < numRows - 1; i++ )
|
|
{
|
|
eigenValues[i] = ( *this )[i][i];
|
|
subd[i] = ( *this )[i + 1][i];
|
|
}
|
|
eigenValues[numRows - 1] = ( *this )[numRows - 1][numRows - 1];
|
|
|
|
Identity();
|
|
|
|
return QL( eigenValues, subd );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SolveSymmetric
|
|
|
|
Determine eigen values and eigen vectors for a symmetric matrix.
|
|
The eigen values are stored in 'eigenValues'.
|
|
Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
|
|
The initial matrix has to be symmetric.
|
|
============
|
|
*/
|
|
bool idMatX::Eigen_SolveSymmetric( idVecX& eigenValues )
|
|
{
|
|
idVecX subd;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
subd.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
eigenValues.SetSize( numRows );
|
|
|
|
HouseholderReduction( eigenValues, subd );
|
|
return QL( eigenValues, subd );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::HessenbergReduction
|
|
|
|
Reduction to Hessenberg form.
|
|
============
|
|
*/
|
|
void idMatX::HessenbergReduction( idMatX& H )
|
|
{
|
|
int i, j, m;
|
|
int low = 0;
|
|
int high = numRows - 1;
|
|
float scale, f, g, h;
|
|
idVecX v;
|
|
|
|
v.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
|
|
for( m = low + 1; m <= high - 1; m++ )
|
|
{
|
|
|
|
scale = 0.0f;
|
|
for( i = m; i <= high; i++ )
|
|
{
|
|
scale = scale + idMath::Fabs( H[i][m - 1] );
|
|
}
|
|
if( scale != 0.0f )
|
|
{
|
|
|
|
// compute Householder transformation.
|
|
h = 0.0f;
|
|
for( i = high; i >= m; i-- )
|
|
{
|
|
v[i] = H[i][m - 1] / scale;
|
|
h += v[i] * v[i];
|
|
}
|
|
g = idMath::Sqrt( h );
|
|
if( v[m] > 0.0f )
|
|
{
|
|
g = -g;
|
|
}
|
|
h = h - v[m] * g;
|
|
v[m] = v[m] - g;
|
|
|
|
// apply Householder similarity transformation
|
|
// H = (I-u*u'/h)*H*(I-u*u')/h)
|
|
for( j = m; j < numRows; j++ )
|
|
{
|
|
f = 0.0f;
|
|
for( i = high; i >= m; i-- )
|
|
{
|
|
f += v[i] * H[i][j];
|
|
}
|
|
f = f / h;
|
|
for( i = m; i <= high; i++ )
|
|
{
|
|
H[i][j] -= f * v[i];
|
|
}
|
|
}
|
|
|
|
for( i = 0; i <= high; i++ )
|
|
{
|
|
f = 0.0f;
|
|
for( j = high; j >= m; j-- )
|
|
{
|
|
f += v[j] * H[i][j];
|
|
}
|
|
f = f / h;
|
|
for( j = m; j <= high; j++ )
|
|
{
|
|
H[i][j] -= f * v[j];
|
|
}
|
|
}
|
|
v[m] = scale * v[m];
|
|
H[m][m - 1] = scale * g;
|
|
}
|
|
}
|
|
|
|
// accumulate transformations
|
|
Identity();
|
|
for( int m = high - 1; m >= low + 1; m-- )
|
|
{
|
|
if( H[m][m - 1] != 0.0f )
|
|
{
|
|
for( i = m + 1; i <= high; i++ )
|
|
{
|
|
v[i] = H[i][m - 1];
|
|
}
|
|
for( j = m; j <= high; j++ )
|
|
{
|
|
g = 0.0f;
|
|
for( i = m; i <= high; i++ )
|
|
{
|
|
g += v[i] * ( *this )[i][j];
|
|
}
|
|
// float division to avoid possible underflow
|
|
g = ( g / v[m] ) / H[m][m - 1];
|
|
for( i = m; i <= high; i++ )
|
|
{
|
|
( *this )[i][j] += g * v[i];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::ComplexDivision
|
|
|
|
Complex scalar division.
|
|
============
|
|
*/
|
|
void idMatX::ComplexDivision( float xr, float xi, float yr, float yi, float& cdivr, float& cdivi )
|
|
{
|
|
float r, d;
|
|
if( idMath::Fabs( yr ) > idMath::Fabs( yi ) )
|
|
{
|
|
r = yi / yr;
|
|
d = yr + r * yi;
|
|
cdivr = ( xr + r * xi ) / d;
|
|
cdivi = ( xi - r * xr ) / d;
|
|
}
|
|
else
|
|
{
|
|
r = yr / yi;
|
|
d = yi + r * yr;
|
|
cdivr = ( r * xr + xi ) / d;
|
|
cdivi = ( r * xi - xr ) / d;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::HessenbergToRealSchur
|
|
|
|
Reduction from Hessenberg to real Schur form.
|
|
============
|
|
*/
|
|
bool idMatX::HessenbergToRealSchur( idMatX& H, idVecX& realEigenValues, idVecX& imaginaryEigenValues )
|
|
{
|
|
int i, j, k;
|
|
int n = numRows - 1;
|
|
int low = 0;
|
|
int high = numRows - 1;
|
|
float eps = 2e-16f, exshift = 0.0f;
|
|
float p = 0.0f, q = 0.0f, r = 0.0f, s = 0.0f, z = 0.0f, t, w, x, y;
|
|
|
|
// store roots isolated by balanc and compute matrix norm
|
|
float norm = 0.0f;
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
if( i < low || i > high )
|
|
{
|
|
realEigenValues[i] = H[i][i];
|
|
imaginaryEigenValues[i] = 0.0f;
|
|
}
|
|
for( j = Max( i - 1, 0 ); j < numRows; j++ )
|
|
{
|
|
norm = norm + idMath::Fabs( H[i][j] );
|
|
}
|
|
}
|
|
|
|
int iter = 0;
|
|
while( n >= low )
|
|
{
|
|
|
|
// look for single small sub-diagonal element
|
|
int l = n;
|
|
while( l > low )
|
|
{
|
|
s = idMath::Fabs( H[l - 1][l - 1] ) + idMath::Fabs( H[l][l] );
|
|
if( s == 0.0f )
|
|
{
|
|
s = norm;
|
|
}
|
|
if( idMath::Fabs( H[l][l - 1] ) < eps * s )
|
|
{
|
|
break;
|
|
}
|
|
l--;
|
|
}
|
|
|
|
// check for convergence
|
|
if( l == n ) // one root found
|
|
{
|
|
H[n][n] = H[n][n] + exshift;
|
|
realEigenValues[n] = H[n][n];
|
|
imaginaryEigenValues[n] = 0.0f;
|
|
n--;
|
|
iter = 0;
|
|
}
|
|
else if( l == n - 1 ) // two roots found
|
|
{
|
|
w = H[n][n - 1] * H[n - 1][n];
|
|
p = ( H[n - 1][n - 1] - H[n][n] ) / 2.0f;
|
|
q = p * p + w;
|
|
z = idMath::Sqrt( idMath::Fabs( q ) );
|
|
H[n][n] = H[n][n] + exshift;
|
|
H[n - 1][n - 1] = H[n - 1][n - 1] + exshift;
|
|
x = H[n][n];
|
|
|
|
if( q >= 0.0f ) // real pair
|
|
{
|
|
if( p >= 0.0f )
|
|
{
|
|
z = p + z;
|
|
}
|
|
else
|
|
{
|
|
z = p - z;
|
|
}
|
|
realEigenValues[n - 1] = x + z;
|
|
realEigenValues[n] = realEigenValues[n - 1];
|
|
if( z != 0.0f )
|
|
{
|
|
realEigenValues[n] = x - w / z;
|
|
}
|
|
imaginaryEigenValues[n - 1] = 0.0f;
|
|
imaginaryEigenValues[n] = 0.0f;
|
|
x = H[n][n - 1];
|
|
s = idMath::Fabs( x ) + idMath::Fabs( z );
|
|
p = x / s;
|
|
q = z / s;
|
|
r = idMath::Sqrt( p * p + q * q );
|
|
p = p / r;
|
|
q = q / r;
|
|
|
|
// modify row
|
|
for( j = n - 1; j < numRows; j++ )
|
|
{
|
|
z = H[n - 1][j];
|
|
H[n - 1][j] = q * z + p * H[n][j];
|
|
H[n][j] = q * H[n][j] - p * z;
|
|
}
|
|
|
|
// modify column
|
|
for( i = 0; i <= n; i++ )
|
|
{
|
|
z = H[i][n - 1];
|
|
H[i][n - 1] = q * z + p * H[i][n];
|
|
H[i][n] = q * H[i][n] - p * z;
|
|
}
|
|
|
|
// accumulate transformations
|
|
for( i = low; i <= high; i++ )
|
|
{
|
|
z = ( *this )[i][n - 1];
|
|
( *this )[i][n - 1] = q * z + p * ( *this )[i][n];
|
|
( *this )[i][n] = q * ( *this )[i][n] - p * z;
|
|
}
|
|
}
|
|
else // complex pair
|
|
{
|
|
realEigenValues[n - 1] = x + p;
|
|
realEigenValues[n] = x + p;
|
|
imaginaryEigenValues[n - 1] = z;
|
|
imaginaryEigenValues[n] = -z;
|
|
}
|
|
n = n - 2;
|
|
iter = 0;
|
|
|
|
}
|
|
else // no convergence yet
|
|
{
|
|
|
|
// form shift
|
|
x = H[n][n];
|
|
y = 0.0f;
|
|
w = 0.0f;
|
|
if( l < n )
|
|
{
|
|
y = H[n - 1][n - 1];
|
|
w = H[n][n - 1] * H[n - 1][n];
|
|
}
|
|
|
|
// Wilkinson's original ad hoc shift
|
|
if( iter == 10 )
|
|
{
|
|
exshift += x;
|
|
for( i = low; i <= n; i++ )
|
|
{
|
|
H[i][i] -= x;
|
|
}
|
|
s = idMath::Fabs( H[n][n - 1] ) + idMath::Fabs( H[n - 1][n - 2] );
|
|
x = y = 0.75f * s;
|
|
w = -0.4375f * s * s;
|
|
}
|
|
|
|
// new ad hoc shift
|
|
if( iter == 30 )
|
|
{
|
|
s = ( y - x ) / 2.0f;
|
|
s = s * s + w;
|
|
if( s > 0 )
|
|
{
|
|
s = idMath::Sqrt( s );
|
|
if( y < x )
|
|
{
|
|
s = -s;
|
|
}
|
|
s = x - w / ( ( y - x ) / 2.0f + s );
|
|
for( i = low; i <= n; i++ )
|
|
{
|
|
H[i][i] -= s;
|
|
}
|
|
exshift += s;
|
|
x = y = w = 0.964f;
|
|
}
|
|
}
|
|
|
|
iter = iter + 1;
|
|
|
|
// look for two consecutive small sub-diagonal elements
|
|
int m;
|
|
for( m = n - 2; m >= l; m-- )
|
|
{
|
|
z = H[m][m];
|
|
r = x - z;
|
|
s = y - z;
|
|
p = ( r * s - w ) / H[m + 1][m] + H[m][m + 1];
|
|
q = H[m + 1][m + 1] - z - r - s;
|
|
r = H[m + 2][m + 1];
|
|
s = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
|
|
p = p / s;
|
|
q = q / s;
|
|
r = r / s;
|
|
if( m == l )
|
|
{
|
|
break;
|
|
}
|
|
if( idMath::Fabs( H[m][m - 1] ) * ( idMath::Fabs( q ) + idMath::Fabs( r ) ) <
|
|
eps * ( idMath::Fabs( p ) * ( idMath::Fabs( H[m - 1][m - 1] ) + idMath::Fabs( z ) + idMath::Fabs( H[m + 1][m + 1] ) ) ) )
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
for( i = m + 2; i <= n; i++ )
|
|
{
|
|
H[i][i - 2] = 0.0f;
|
|
if( i > m + 2 )
|
|
{
|
|
H[i][i - 3] = 0.0f;
|
|
}
|
|
}
|
|
|
|
// double QR step involving rows l:n and columns m:n
|
|
for( k = m; k <= n - 1; k++ )
|
|
{
|
|
bool notlast = ( k != n - 1 );
|
|
if( k != m )
|
|
{
|
|
p = H[k][k - 1];
|
|
q = H[k + 1][k - 1];
|
|
r = ( notlast ? H[k + 2][k - 1] : 0.0f );
|
|
x = idMath::Fabs( p ) + idMath::Fabs( q ) + idMath::Fabs( r );
|
|
if( x != 0.0f )
|
|
{
|
|
p = p / x;
|
|
q = q / x;
|
|
r = r / x;
|
|
}
|
|
}
|
|
if( x == 0.0f )
|
|
{
|
|
break;
|
|
}
|
|
s = idMath::Sqrt( p * p + q * q + r * r );
|
|
if( p < 0.0f )
|
|
{
|
|
s = -s;
|
|
}
|
|
if( s != 0.0f )
|
|
{
|
|
if( k != m )
|
|
{
|
|
H[k][k - 1] = -s * x;
|
|
}
|
|
else if( l != m )
|
|
{
|
|
H[k][k - 1] = -H[k][k - 1];
|
|
}
|
|
p = p + s;
|
|
x = p / s;
|
|
y = q / s;
|
|
z = r / s;
|
|
q = q / p;
|
|
r = r / p;
|
|
|
|
// modify row
|
|
for( j = k; j < numRows; j++ )
|
|
{
|
|
p = H[k][j] + q * H[k + 1][j];
|
|
if( notlast )
|
|
{
|
|
p = p + r * H[k + 2][j];
|
|
H[k + 2][j] = H[k + 2][j] - p * z;
|
|
}
|
|
H[k][j] = H[k][j] - p * x;
|
|
H[k + 1][j] = H[k + 1][j] - p * y;
|
|
}
|
|
|
|
// modify column
|
|
for( i = 0; i <= Min( n, k + 3 ); i++ )
|
|
{
|
|
p = x * H[i][k] + y * H[i][k + 1];
|
|
if( notlast )
|
|
{
|
|
p = p + z * H[i][k + 2];
|
|
H[i][k + 2] = H[i][k + 2] - p * r;
|
|
}
|
|
H[i][k] = H[i][k] - p;
|
|
H[i][k + 1] = H[i][k + 1] - p * q;
|
|
}
|
|
|
|
// accumulate transformations
|
|
for( i = low; i <= high; i++ )
|
|
{
|
|
p = x * ( *this )[i][k] + y * ( *this )[i][k + 1];
|
|
if( notlast )
|
|
{
|
|
p = p + z * ( *this )[i][k + 2];
|
|
( *this )[i][k + 2] = ( *this )[i][k + 2] - p * r;
|
|
}
|
|
( *this )[i][k] = ( *this )[i][k] - p;
|
|
( *this )[i][k + 1] = ( *this )[i][k + 1] - p * q;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// backsubstitute to find vectors of upper triangular form
|
|
if( norm == 0.0f )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
for( n = numRows - 1; n >= 0; n-- )
|
|
{
|
|
p = realEigenValues[n];
|
|
q = imaginaryEigenValues[n];
|
|
|
|
if( q == 0.0f ) // real vector
|
|
{
|
|
int l = n;
|
|
H[n][n] = 1.0f;
|
|
for( i = n - 1; i >= 0; i-- )
|
|
{
|
|
w = H[i][i] - p;
|
|
r = 0.0f;
|
|
for( j = l; j <= n; j++ )
|
|
{
|
|
r = r + H[i][j] * H[j][n];
|
|
}
|
|
if( imaginaryEigenValues[i] < 0.0f )
|
|
{
|
|
z = w;
|
|
s = r;
|
|
}
|
|
else
|
|
{
|
|
l = i;
|
|
if( imaginaryEigenValues[i] == 0.0f )
|
|
{
|
|
if( w != 0.0f )
|
|
{
|
|
H[i][n] = -r / w;
|
|
}
|
|
else
|
|
{
|
|
H[i][n] = -r / ( eps * norm );
|
|
}
|
|
}
|
|
else // solve real equations
|
|
{
|
|
x = H[i][i + 1];
|
|
y = H[i + 1][i];
|
|
q = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i];
|
|
t = ( x * s - z * r ) / q;
|
|
H[i][n] = t;
|
|
if( idMath::Fabs( x ) > idMath::Fabs( z ) )
|
|
{
|
|
H[i + 1][n] = ( -r - w * t ) / x;
|
|
}
|
|
else
|
|
{
|
|
H[i + 1][n] = ( -s - y * t ) / z;
|
|
}
|
|
}
|
|
|
|
// overflow control
|
|
t = idMath::Fabs( H[i][n] );
|
|
if( ( eps * t ) * t > 1 )
|
|
{
|
|
for( j = i; j <= n; j++ )
|
|
{
|
|
H[j][n] = H[j][n] / t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if( q < 0.0f ) // complex vector
|
|
{
|
|
int l = n - 1;
|
|
|
|
// last vector component imaginary so matrix is triangular
|
|
if( idMath::Fabs( H[n][n - 1] ) > idMath::Fabs( H[n - 1][n] ) )
|
|
{
|
|
H[n - 1][n - 1] = q / H[n][n - 1];
|
|
H[n - 1][n] = -( H[n][n] - p ) / H[n][n - 1];
|
|
}
|
|
else
|
|
{
|
|
ComplexDivision( 0.0f, -H[n - 1][n], H[n - 1][n - 1] - p, q, H[n - 1][n - 1], H[n - 1][n] );
|
|
}
|
|
H[n][n - 1] = 0.0f;
|
|
H[n][n] = 1.0f;
|
|
for( i = n - 2; i >= 0; i-- )
|
|
{
|
|
float ra, sa, vr, vi;
|
|
ra = 0.0f;
|
|
sa = 0.0f;
|
|
for( j = l; j <= n; j++ )
|
|
{
|
|
ra = ra + H[i][j] * H[j][n - 1];
|
|
sa = sa + H[i][j] * H[j][n];
|
|
}
|
|
w = H[i][i] - p;
|
|
|
|
if( imaginaryEigenValues[i] < 0.0f )
|
|
{
|
|
z = w;
|
|
r = ra;
|
|
s = sa;
|
|
}
|
|
else
|
|
{
|
|
l = i;
|
|
if( imaginaryEigenValues[i] == 0.0f )
|
|
{
|
|
ComplexDivision( -ra, -sa, w, q, H[i][n - 1], H[i][n] );
|
|
}
|
|
else
|
|
{
|
|
// solve complex equations
|
|
x = H[i][i + 1];
|
|
y = H[i + 1][i];
|
|
vr = ( realEigenValues[i] - p ) * ( realEigenValues[i] - p ) + imaginaryEigenValues[i] * imaginaryEigenValues[i] - q * q;
|
|
vi = ( realEigenValues[i] - p ) * 2.0f * q;
|
|
if( vr == 0.0f && vi == 0.0f )
|
|
{
|
|
vr = eps * norm * ( idMath::Fabs( w ) + idMath::Fabs( q ) + idMath::Fabs( x ) + idMath::Fabs( y ) + idMath::Fabs( z ) );
|
|
}
|
|
ComplexDivision( x * r - z * ra + q * sa, x * s - z * sa - q * ra, vr, vi, H[i][n - 1], H[i][n] );
|
|
if( idMath::Fabs( x ) > ( idMath::Fabs( z ) + idMath::Fabs( q ) ) )
|
|
{
|
|
H[i + 1][n - 1] = ( -ra - w * H[i][n - 1] + q * H[i][n] ) / x;
|
|
H[i + 1][n] = ( -sa - w * H[i][n] - q * H[i][n - 1] ) / x;
|
|
}
|
|
else
|
|
{
|
|
ComplexDivision( -r - y * H[i][n - 1], -s - y * H[i][n], z, q, H[i + 1][n - 1], H[i + 1][n] );
|
|
}
|
|
}
|
|
|
|
// overflow control
|
|
t = Max( idMath::Fabs( H[i][n - 1] ), idMath::Fabs( H[i][n] ) );
|
|
if( ( eps * t ) * t > 1 )
|
|
{
|
|
for( j = i; j <= n; j++ )
|
|
{
|
|
H[j][n - 1] = H[j][n - 1] / t;
|
|
H[j][n] = H[j][n] / t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// vectors of isolated roots
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
if( i < low || i > high )
|
|
{
|
|
for( j = i; j < numRows; j++ )
|
|
{
|
|
( *this )[i][j] = H[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
// back transformation to get eigenvectors of original matrix
|
|
for( j = numRows - 1; j >= low; j-- )
|
|
{
|
|
for( i = low; i <= high; i++ )
|
|
{
|
|
z = 0.0f;
|
|
for( k = low; k <= Min( j, high ); k++ )
|
|
{
|
|
z = z + ( *this )[i][k] * H[k][j];
|
|
}
|
|
( *this )[i][j] = z;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_Solve
|
|
|
|
Determine eigen values and eigen vectors for a square matrix.
|
|
The eigen values are stored in 'realEigenValues' and 'imaginaryEigenValues'.
|
|
Column i of the original matrix will store the eigen vector corresponding to the realEigenValues[i] and imaginaryEigenValues[i].
|
|
============
|
|
*/
|
|
bool idMatX::Eigen_Solve( idVecX& realEigenValues, idVecX& imaginaryEigenValues )
|
|
{
|
|
idMatX H;
|
|
|
|
assert( numRows == numColumns );
|
|
|
|
realEigenValues.SetSize( numRows );
|
|
imaginaryEigenValues.SetSize( numRows );
|
|
|
|
H = *this;
|
|
|
|
// reduce to Hessenberg form
|
|
HessenbergReduction( H );
|
|
|
|
// reduce Hessenberg to real Schur form
|
|
return HessenbergToRealSchur( H, realEigenValues, imaginaryEigenValues );
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SortIncreasing
|
|
============
|
|
*/
|
|
void idMatX::Eigen_SortIncreasing( idVecX& eigenValues )
|
|
{
|
|
for( int i = 0, j = 0; i <= numRows - 2; i++ )
|
|
{
|
|
j = i;
|
|
float min = eigenValues[j];
|
|
for( int k = i + 1; k < numRows; k++ )
|
|
{
|
|
if( eigenValues[k] < min )
|
|
{
|
|
j = k;
|
|
min = eigenValues[j];
|
|
}
|
|
}
|
|
if( j != i )
|
|
{
|
|
eigenValues.SwapElements( i, j );
|
|
SwapColumns( i, j );
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SortDecreasing
|
|
============
|
|
*/
|
|
void idMatX::Eigen_SortDecreasing( idVecX& eigenValues )
|
|
{
|
|
for( int i = 0, j = 0; i <= numRows - 2; i++ )
|
|
{
|
|
j = i;
|
|
float max = eigenValues[j];
|
|
for( int k = i + 1; k < numRows; k++ )
|
|
{
|
|
if( eigenValues[k] > max )
|
|
{
|
|
j = k;
|
|
max = eigenValues[j];
|
|
}
|
|
}
|
|
if( j != i )
|
|
{
|
|
eigenValues.SwapElements( i, j );
|
|
SwapColumns( i, j );
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::DeterminantGeneric
|
|
============
|
|
*/
|
|
float idMatX::DeterminantGeneric() const
|
|
{
|
|
int* index;
|
|
float det;
|
|
idMatX tmp;
|
|
|
|
index = ( int* ) _alloca16( numRows * sizeof( int ) );
|
|
tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
|
|
tmp = *this;
|
|
|
|
if( !tmp.LU_Factor( index, &det ) )
|
|
{
|
|
return 0.0f;
|
|
}
|
|
|
|
return det;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::InverseSelfGeneric
|
|
============
|
|
*/
|
|
bool idMatX::InverseSelfGeneric()
|
|
{
|
|
int i, j, *index;
|
|
idMatX tmp;
|
|
idVecX x, b;
|
|
|
|
index = ( int* ) _alloca16( numRows * sizeof( int ) );
|
|
tmp.SetData( numRows, numColumns, MATX_ALLOCA( numRows * numColumns ) );
|
|
tmp = *this;
|
|
|
|
if( !tmp.LU_Factor( index ) )
|
|
{
|
|
return false;
|
|
}
|
|
|
|
x.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.SetData( numRows, VECX_ALLOCA( numRows ) );
|
|
b.Zero();
|
|
|
|
for( i = 0; i < numRows; i++ )
|
|
{
|
|
|
|
b[i] = 1.0f;
|
|
tmp.LU_Solve( x, b, index );
|
|
for( j = 0; j < numRows; j++ )
|
|
{
|
|
( *this )[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Test
|
|
============
|
|
*/
|
|
void idMatX::Test()
|
|
{
|
|
idMatX original, m1, m2, m3, q1, q2, r1, r2;
|
|
idVecX v, w, u, c, d;
|
|
int offset, size, *index1, *index2;
|
|
|
|
size = 6;
|
|
original.Random( size, size, 0 );
|
|
original = original * original.Transpose();
|
|
|
|
index1 = ( int* ) _alloca16( ( size + 1 ) * sizeof( index1[0] ) );
|
|
index2 = ( int* ) _alloca16( ( size + 1 ) * sizeof( index2[0] ) );
|
|
|
|
/*
|
|
idMatX::LowerTriangularInverse
|
|
*/
|
|
|
|
m1 = original;
|
|
m1.ClearUpperTriangle();
|
|
m2 = m1;
|
|
|
|
m2.InverseSelf();
|
|
m1.LowerTriangularInverse();
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LowerTriangularInverse failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::UpperTriangularInverse
|
|
*/
|
|
|
|
m1 = original;
|
|
m1.ClearLowerTriangle();
|
|
m2 = m1;
|
|
|
|
m2.InverseSelf();
|
|
m1.UpperTriangularInverse();
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::UpperTriangularInverse failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_GaussJordan
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.Inverse_GaussJordan();
|
|
m1 *= original;
|
|
|
|
if( !m1.IsIdentity( 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Inverse_GaussJordan failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random( size, 1 );
|
|
v.Random( size, 2 );
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_RankOne( v, w, 1.0f );
|
|
if( !m2.Inverse_GaussJordan() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateRankOne( v, w, 1.0f );
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Inverse_UpdateRankOne failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateRowColumn
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random( size, 1 );
|
|
w.Random( size, 2 );
|
|
w[offset] = 0.0f;
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_RowColumn( v, w, offset );
|
|
if( !m2.Inverse_GaussJordan() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateRowColumn( v, w, offset );
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Inverse_UpdateRowColumn failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateIncrement
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random( size + 1, 1 );
|
|
w.Random( size + 1, 2 );
|
|
w[size] = 0.0f;
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_Increment( v, w );
|
|
if( !m2.Inverse_GaussJordan() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateIncrement( v, w );
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Inverse_UpdateIncrement failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateDecrement
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize( 6 );
|
|
w.SetSize( 6 );
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
v[i] = original[i][offset];
|
|
w[i] = original[offset][i];
|
|
}
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_Decrement( offset );
|
|
if( !m2.Inverse_GaussJordan() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateDecrement( v, w, offset );
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Inverse_UpdateDecrement failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.LU_Factor( NULL ); // no pivoting
|
|
m1.LU_UnpackFactors( m2, m3 );
|
|
m1 = m2 * m3;
|
|
|
|
if( !original.Compare( m1, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LU_Factor failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random( size, 1 );
|
|
v.Random( size, 2 );
|
|
|
|
// factor m1
|
|
m1.LU_Factor( index1 );
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOne( v, w, 1.0f );
|
|
if( !m2.LU_Factor( index2 ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.LU_MultiplyFactors( m3, index2 );
|
|
m2 = m3;
|
|
|
|
// update factored m1
|
|
m1.LU_UpdateRankOne( v, w, 1.0f, index1 );
|
|
m1.LU_MultiplyFactors( m3, index1 );
|
|
m1 = m3;
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LU_UpdateRankOne failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateRowColumn
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random( size, 1 );
|
|
w.Random( size, 2 );
|
|
w[offset] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.LU_Factor( index1 );
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumn( v, w, offset );
|
|
if( !m2.LU_Factor( index2 ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.LU_MultiplyFactors( m3, index2 );
|
|
m2 = m3;
|
|
|
|
// update m1
|
|
m1.LU_UpdateRowColumn( v, w, offset, index1 );
|
|
m1.LU_MultiplyFactors( m3, index1 );
|
|
m1 = m3;
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LU_UpdateRowColumn failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateIncrement
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random( size + 1, 1 );
|
|
w.Random( size + 1, 2 );
|
|
w[size] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.LU_Factor( index1 );
|
|
|
|
// modify and factor m2
|
|
m2.Update_Increment( v, w );
|
|
if( !m2.LU_Factor( index2 ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.LU_MultiplyFactors( m3, index2 );
|
|
m2 = m3;
|
|
|
|
// update factored m1
|
|
m1.LU_UpdateIncrement( v, w, index1 );
|
|
m1.LU_MultiplyFactors( m3, index1 );
|
|
m1 = m3;
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LU_UpdateIncrement failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateDecrement
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize( 6 );
|
|
w.SetSize( 6 );
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
v[i] = original[i][offset];
|
|
w[i] = original[offset][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.LU_Factor( index1 );
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement( offset );
|
|
if( !m2.LU_Factor( index2 ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.LU_MultiplyFactors( m3, index2 );
|
|
m2 = m3;
|
|
|
|
u.SetSize( 6 );
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
u[i] = original[index1[offset]][i];
|
|
}
|
|
|
|
// update factors of m1
|
|
m1.LU_UpdateDecrement( v, w, u, offset, index1 );
|
|
m1.LU_MultiplyFactors( m3, index1 );
|
|
m1 = m3;
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LU_UpdateDecrement failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.LU_Factor( NULL );
|
|
m2.LU_Inverse( m1, NULL );
|
|
m1 *= original;
|
|
|
|
if( !m1.IsIdentity( 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LU_Inverse failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_Factor
|
|
*/
|
|
|
|
c.SetSize( size );
|
|
d.SetSize( size );
|
|
|
|
m1 = original;
|
|
|
|
m1.QR_Factor( c, d );
|
|
m1.QR_UnpackFactors( q1, r1, c, d );
|
|
m1 = q1 * r1;
|
|
|
|
if( !original.Compare( m1, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::QR_Factor failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateRankOne
|
|
*/
|
|
|
|
c.SetSize( size );
|
|
d.SetSize( size );
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random( size, 0 );
|
|
v = w;
|
|
|
|
// factor m1
|
|
m1.QR_Factor( c, d );
|
|
m1.QR_UnpackFactors( q1, r1, c, d );
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOne( v, w, 1.0f );
|
|
if( !m2.QR_Factor( c, d ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.QR_UnpackFactors( q2, r2, c, d );
|
|
m2 = q2 * r2;
|
|
|
|
// update factored m1
|
|
q1.QR_UpdateRankOne( r1, v, w, 1.0f );
|
|
m1 = q1 * r1;
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::QR_UpdateRankOne failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateRowColumn
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
c.SetSize( size );
|
|
d.SetSize( size );
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random( size, 1 );
|
|
w.Random( size, 2 );
|
|
w[offset] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.QR_Factor( c, d );
|
|
m1.QR_UnpackFactors( q1, r1, c, d );
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumn( v, w, offset );
|
|
if( !m2.QR_Factor( c, d ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.QR_UnpackFactors( q2, r2, c, d );
|
|
m2 = q2 * r2;
|
|
|
|
// update m1
|
|
q1.QR_UpdateRowColumn( r1, v, w, offset );
|
|
m1 = q1 * r1;
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::QR_UpdateRowColumn failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateIncrement
|
|
*/
|
|
|
|
c.SetSize( size + 1 );
|
|
d.SetSize( size + 1 );
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random( size + 1, 1 );
|
|
w.Random( size + 1, 2 );
|
|
w[size] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.QR_Factor( c, d );
|
|
m1.QR_UnpackFactors( q1, r1, c, d );
|
|
|
|
// modify and factor m2
|
|
m2.Update_Increment( v, w );
|
|
if( !m2.QR_Factor( c, d ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.QR_UnpackFactors( q2, r2, c, d );
|
|
m2 = q2 * r2;
|
|
|
|
// update factored m1
|
|
q1.QR_UpdateIncrement( r1, v, w );
|
|
m1 = q1 * r1;
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::QR_UpdateIncrement failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateDecrement
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
c.SetSize( size + 1 );
|
|
d.SetSize( size + 1 );
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize( 6 );
|
|
w.SetSize( 6 );
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
v[i] = original[i][offset];
|
|
w[i] = original[offset][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.QR_Factor( c, d );
|
|
m1.QR_UnpackFactors( q1, r1, c, d );
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement( offset );
|
|
if( !m2.QR_Factor( c, d ) )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.QR_UnpackFactors( q2, r2, c, d );
|
|
m2 = q2 * r2;
|
|
|
|
// update factors of m1
|
|
q1.QR_UpdateDecrement( r1, v, w, offset );
|
|
m1 = q1 * r1;
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::QR_UpdateDecrement failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.QR_Factor( c, d );
|
|
m2.QR_Inverse( m1, c, d );
|
|
m1 *= original;
|
|
|
|
if( !m1.IsIdentity( 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::QR_Inverse failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::SVD_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
m3.Zero( size, size );
|
|
w.Zero( size );
|
|
|
|
m1.SVD_Factor( w, m3 );
|
|
m2.Diag( w );
|
|
m3.TransposeSelf();
|
|
m1 = m1 * m2 * m3;
|
|
|
|
if( !original.Compare( m1, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::SVD_Factor failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::SVD_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.SVD_Factor( w, m3 );
|
|
m2.SVD_Inverse( m1, w, m3 );
|
|
m1 *= original;
|
|
|
|
if( !m1.IsIdentity( 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::SVD_Inverse failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.Cholesky_Factor();
|
|
m1.Cholesky_MultiplyFactors( m2 );
|
|
|
|
if( !original.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Cholesky_Factor failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random( size, 0 );
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOneSymmetric( w, 1.0f );
|
|
if( !m2.Cholesky_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update factored m1
|
|
m1.Cholesky_UpdateRankOne( w, 1.0f, 0 );
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Cholesky_UpdateRankOne failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateRowColumn
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
int pdtable[] = { 1, 0, 1, 0, 0, 0 };
|
|
w.Random( size, pdtable[offset] );
|
|
w *= 0.1f;
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumnSymmetric( w, offset );
|
|
if( !m2.Cholesky_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update m1
|
|
m1.Cholesky_UpdateRowColumn( w, offset );
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Cholesky_UpdateRowColumn failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateIncrement
|
|
*/
|
|
|
|
m1.Random( size + 1, size + 1, 0 );
|
|
m3 = m1 * m1.Transpose();
|
|
|
|
m1.SquareSubMatrix( m3, size );
|
|
m2 = m1;
|
|
|
|
w.SetSize( size + 1 );
|
|
for( int i = 0; i < size + 1; i++ )
|
|
{
|
|
w[i] = m3[size][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_IncrementSymmetric( w );
|
|
if( !m2.Cholesky_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update factored m1
|
|
m1.Cholesky_UpdateIncrement( w );
|
|
|
|
m1.ClearUpperTriangle();
|
|
m2.ClearUpperTriangle();
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Cholesky_UpdateIncrement failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateDecrement
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset += size - 1 )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize( 6 );
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
v[i] = original[i][offset];
|
|
}
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement( offset );
|
|
if( !m2.Cholesky_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update factors of m1
|
|
m1.Cholesky_UpdateDecrement( v, offset );
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Cholesky_UpdateDecrement failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.Cholesky_Factor();
|
|
m2.Cholesky_Inverse( m1 );
|
|
m1 *= original;
|
|
|
|
if( !m1.IsIdentity( 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Cholesky_Inverse failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.LDLT_Factor();
|
|
m1.LDLT_MultiplyFactors( m2 );
|
|
|
|
if( !original.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LDLT_Factor failed" );
|
|
}
|
|
|
|
m1.LDLT_UnpackFactors( m2, m3 );
|
|
m2 = m2 * m3 * m2.Transpose();
|
|
|
|
if( !original.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LDLT_Factor failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random( size, 0 );
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOneSymmetric( w, 1.0f );
|
|
if( !m2.LDLT_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update factored m1
|
|
m1.LDLT_UpdateRankOne( w, 1.0f, 0 );
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LDLT_UpdateRankOne failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateRowColumn
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random( size, 0 );
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumnSymmetric( w, offset );
|
|
if( !m2.LDLT_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update m1
|
|
m1.LDLT_UpdateRowColumn( w, offset );
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LDLT_UpdateRowColumn failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateIncrement
|
|
*/
|
|
|
|
m1.Random( size + 1, size + 1, 0 );
|
|
m3 = m1 * m1.Transpose();
|
|
|
|
m1.SquareSubMatrix( m3, size );
|
|
m2 = m1;
|
|
|
|
w.SetSize( size + 1 );
|
|
for( int i = 0; i < size + 1; i++ )
|
|
{
|
|
w[i] = m3[size][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_IncrementSymmetric( w );
|
|
if( !m2.LDLT_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update factored m1
|
|
m1.LDLT_UpdateIncrement( w );
|
|
|
|
m1.ClearUpperTriangle();
|
|
m2.ClearUpperTriangle();
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LDLT_UpdateIncrement failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateDecrement
|
|
*/
|
|
|
|
for( offset = 0; offset < size; offset++ )
|
|
{
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize( 6 );
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
v[i] = original[i][offset];
|
|
}
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement( offset );
|
|
if( !m2.LDLT_Factor() )
|
|
{
|
|
assert( 0 );
|
|
}
|
|
|
|
// update factors of m1
|
|
m1.LDLT_UpdateDecrement( v, offset );
|
|
|
|
if( !m1.Compare( m2, 1e-3f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LDLT_UpdateDecrement failed" );
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.LDLT_Factor();
|
|
m2.LDLT_Inverse( m1 );
|
|
m1 *= original;
|
|
|
|
if( !m1.IsIdentity( 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::LDLT_Inverse failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Eigen_SolveSymmetricTriDiagonal
|
|
*/
|
|
|
|
m3 = original;
|
|
m3.TriDiagonal_ClearTriangles();
|
|
m1 = m3;
|
|
|
|
v.SetSize( size );
|
|
|
|
m1.Eigen_SolveSymmetricTriDiagonal( v );
|
|
|
|
m3.TransposeMultiply( m2, m1 );
|
|
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
for( int j = 0; j < size; j++ )
|
|
{
|
|
m1[i][j] *= v[j];
|
|
}
|
|
}
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Eigen_SolveSymmetricTriDiagonal failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Eigen_SolveSymmetric
|
|
*/
|
|
|
|
m3 = original;
|
|
m1 = m3;
|
|
|
|
v.SetSize( size );
|
|
|
|
m1.Eigen_SolveSymmetric( v );
|
|
|
|
m3.TransposeMultiply( m2, m1 );
|
|
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
for( int j = 0; j < size; j++ )
|
|
{
|
|
m1[i][j] *= v[j];
|
|
}
|
|
}
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Eigen_SolveSymmetric failed" );
|
|
}
|
|
|
|
/*
|
|
idMatX::Eigen_Solve
|
|
*/
|
|
|
|
m3 = original;
|
|
m1 = m3;
|
|
|
|
v.SetSize( size );
|
|
w.SetSize( size );
|
|
|
|
m1.Eigen_Solve( v, w );
|
|
|
|
m3.TransposeMultiply( m2, m1 );
|
|
|
|
for( int i = 0; i < size; i++ )
|
|
{
|
|
for( int j = 0; j < size; j++ )
|
|
{
|
|
m1[i][j] *= v[j];
|
|
}
|
|
}
|
|
|
|
if( !m1.Compare( m2, 1e-4f ) )
|
|
{
|
|
idLib::common->Warning( "idMatX::Eigen_Solve failed" );
|
|
}
|
|
}
|