761 lines
16 KiB
C++
761 lines
16 KiB
C++
|
//+-------------------------------------------------------------------------
|
||
|
|
//
|
||
|
|
// Microsoft Windows
|
||
|
|
//
|
||
|
|
// Copyright (C) Microsoft Corporation, 1997 - 1997
|
||
|
|
//
|
||
|
|
// File: vrmatrx.cpp
|
||
|
|
//
|
||
|
|
//--------------------------------------------------------------------------
|
||
|
|
|
||
|
|
#include <float.h>
|
||
|
|
#include <math.h>
|
||
|
|
#include <bitset>
|
||
|
|
#include "vrmatrx.h"
|
||
|
|
|
||
|
|
VRMATRIX VRMATRIX :: VrmatrixProject ( const VIMD & vimdRowColumnRetain ) const
|
||
|
|
{
|
||
|
|
// Returns the projection of this matrix defined by the rows and columns
|
||
|
|
// in vimdRowColumnRetain.
|
||
|
|
|
||
|
|
#define BSETSIZE 100
|
||
|
|
|
||
|
|
size_t cDimMax = _cpp_max(CCol(),CRow());
|
||
|
|
assert( cDimMax < BSETSIZE );
|
||
|
|
|
||
|
|
// Build a bitset that keeps track of the rows and columns we're retaining
|
||
|
|
|
||
|
|
bitset<BSETSIZE> bset;
|
||
|
|
|
||
|
|
for ( int iRowCol = 0; iRowCol < vimdRowColumnRetain.size(); ++iRowCol)
|
||
|
|
{
|
||
|
|
bset[ vimdRowColumnRetain[iRowCol] ] = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
int cCol = 0;
|
||
|
|
int cRow = 0;
|
||
|
|
|
||
|
|
for ( iRowCol = 0; iRowCol < cDimMax; iRowCol++ )
|
||
|
|
{
|
||
|
|
bool bKeep = bset[iRowCol];
|
||
|
|
|
||
|
|
if ( cDimMax >= CCol() && bKeep )
|
||
|
|
cCol++;
|
||
|
|
if ( cDimMax >= CRow() && bKeep )
|
||
|
|
cRow++;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Make sure that a least one row and column are being retained
|
||
|
|
if ( cCol == 0 || cRow == 0 )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"null matrix projection");
|
||
|
|
|
||
|
|
// Construct the projection matrix
|
||
|
|
VRMATRIX vrmatrix(cRow,cCol);
|
||
|
|
|
||
|
|
int iRowProjection = 0;
|
||
|
|
|
||
|
|
// Step through every element in this matrix, and insert into the
|
||
|
|
// projection if the element is to be retained
|
||
|
|
|
||
|
|
for ( int iRow = 0; iRow < CRow(); ++iRow )
|
||
|
|
{
|
||
|
|
if ( ! bset[iRow] )
|
||
|
|
{
|
||
|
|
// This row is excluded from the projection
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
int iColProjection = 0;
|
||
|
|
|
||
|
|
// This row is included... insert the members
|
||
|
|
// of the row for every column in the projection
|
||
|
|
|
||
|
|
for (int iCol = 0; iCol < CCol(); ++iCol )
|
||
|
|
{
|
||
|
|
if ( bset[iCol] )
|
||
|
|
{
|
||
|
|
vrmatrix(iRowProjection, iColProjection) = self(iRow,iCol);
|
||
|
|
|
||
|
|
++iColProjection;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
++iRowProjection;
|
||
|
|
}
|
||
|
|
return vrmatrix;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIXSQ VRMATRIXSQ :: VrmatrixProject ( const VIMD & vimdRowColumnRetain ) const
|
||
|
|
{
|
||
|
|
// Returns the projection of this matrix defined by the rows and columns
|
||
|
|
// in vimdRowColumnRetain.
|
||
|
|
|
||
|
|
#define BSETSIZE 100
|
||
|
|
|
||
|
|
size_t cDimMax = _cpp_max(CCol(),CRow());
|
||
|
|
assert( cDimMax < BSETSIZE );
|
||
|
|
|
||
|
|
// Build a bitset that keeps track of the rows and columns we're retaining
|
||
|
|
bitset<BSETSIZE> bset;
|
||
|
|
|
||
|
|
for ( int iRowCol = 0; iRowCol < vimdRowColumnRetain.size(); ++iRowCol)
|
||
|
|
{
|
||
|
|
bset[ vimdRowColumnRetain[iRowCol] ] = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
int cCol = 0;
|
||
|
|
int cRow = 0;
|
||
|
|
|
||
|
|
for ( iRowCol = 0; iRowCol < cDimMax; iRowCol++ )
|
||
|
|
{
|
||
|
|
bool bKeep = bset[iRowCol];
|
||
|
|
|
||
|
|
if ( cDimMax >= CCol() && bKeep )
|
||
|
|
cCol++;
|
||
|
|
if ( cDimMax >= CRow() && bKeep )
|
||
|
|
cRow++;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIXSQ vrmatrix;
|
||
|
|
|
||
|
|
// Make sure that a least one row and column are being retained
|
||
|
|
if ( cCol > 0 && cRow > 0 )
|
||
|
|
{
|
||
|
|
// Initialize the projection matrix
|
||
|
|
vrmatrix.Init(cRow,cCol);
|
||
|
|
|
||
|
|
int iRowProjection = 0;
|
||
|
|
|
||
|
|
// Step through every element in this matrix, and insert into the
|
||
|
|
// projection if the element is to be retained
|
||
|
|
|
||
|
|
for ( int iRow = 0; iRow < CRow(); ++iRow )
|
||
|
|
{
|
||
|
|
if ( ! bset[iRow] )
|
||
|
|
{
|
||
|
|
// This row is excluded from the projection
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
int iColProjection = 0;
|
||
|
|
|
||
|
|
// This row is included... insert the members
|
||
|
|
// of the row for every column in the projection
|
||
|
|
|
||
|
|
for (int iCol = 0; iCol < CCol(); ++iCol )
|
||
|
|
{
|
||
|
|
if ( bset[iCol] )
|
||
|
|
{
|
||
|
|
vrmatrix(iRowProjection, iColProjection) = self(iRow,iCol);
|
||
|
|
|
||
|
|
++iColProjection;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
++iRowProjection;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
vrmatrix.Init(0,0);
|
||
|
|
}
|
||
|
|
return vrmatrix;
|
||
|
|
}
|
||
|
|
|
||
|
|
VLREAL VRMATRIX :: VectorRow ( int iRow ) const
|
||
|
|
{
|
||
|
|
// Return a copy of the iRow'th row vector of the matrix
|
||
|
|
|
||
|
|
if ( iRow >= CRow() )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"invalid matrix projection");
|
||
|
|
|
||
|
|
VLREAL vectorRowReturn;
|
||
|
|
|
||
|
|
int cCol = CCol();
|
||
|
|
|
||
|
|
vectorRowReturn.resize(cCol);
|
||
|
|
|
||
|
|
const REAL* rgrealRowMatrix = & self(iRow,0);
|
||
|
|
|
||
|
|
for ( int iCol = 0; iCol < cCol; cCol++ )
|
||
|
|
{
|
||
|
|
vectorRowReturn[iCol] = rgrealRowMatrix[iCol];
|
||
|
|
}
|
||
|
|
// *prv++ = *prm++;
|
||
|
|
|
||
|
|
return vectorRowReturn;
|
||
|
|
}
|
||
|
|
|
||
|
|
VLREAL VRMATRIX :: VectorColumn ( int iCol ) const
|
||
|
|
{
|
||
|
|
// Return a copy of the iCol'th column vector of the matrix
|
||
|
|
|
||
|
|
if ( iCol >= CCol() )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"invalid matrix projection");
|
||
|
|
|
||
|
|
VLREAL vectorColReturn;
|
||
|
|
|
||
|
|
int cRow = CRow();
|
||
|
|
|
||
|
|
vectorColReturn.resize(cRow);
|
||
|
|
|
||
|
|
const REAL* rgrealColMatrix = & self(0, iCol);
|
||
|
|
|
||
|
|
for ( int iRow = 0; iRow < cRow; iRow++ )
|
||
|
|
{
|
||
|
|
vectorColReturn[iRow] = rgrealColMatrix[iRow];
|
||
|
|
}
|
||
|
|
|
||
|
|
return vectorColReturn;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX VRMATRIX :: VrmatrixTranspose () const
|
||
|
|
{
|
||
|
|
// Return the transpose of this matrix
|
||
|
|
|
||
|
|
VRMATRIX vrmatrixTranspose( CCol(), CRow() );
|
||
|
|
|
||
|
|
for ( int iRow = 0 ; iRow < CRow() ; iRow++ )
|
||
|
|
{
|
||
|
|
for ( int iCol = 0; iCol < CCol(); iCol++ )
|
||
|
|
{
|
||
|
|
vrmatrixTranspose(iCol,iRow) = self(iRow,iCol);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return vrmatrixTranspose;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX VRMATRIX::operator * ( const VRMATRIX & matrix ) const
|
||
|
|
{
|
||
|
|
if ( ! BCanMultiply( matrix ) )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"invalid matrix multiplication");
|
||
|
|
|
||
|
|
// Result matrix
|
||
|
|
VRMATRIX mat( CRow(), matrix.CCol() );
|
||
|
|
|
||
|
|
// Compute distance in flat array between adjacent
|
||
|
|
// column items in secondary
|
||
|
|
int icolInc = matrix.second.stride()[0];
|
||
|
|
|
||
|
|
const REAL * prrow = & self(0,0);
|
||
|
|
REAL * prmat = & mat(0,0);
|
||
|
|
for (int irow = 0; irow < CRow(); irow++)
|
||
|
|
{
|
||
|
|
const REAL * prrowt;
|
||
|
|
for ( int icol = 0; icol < matrix.CCol(); icol++ )
|
||
|
|
{
|
||
|
|
prrowt = prrow;
|
||
|
|
assert( prrowt == & self(irow,0) );
|
||
|
|
|
||
|
|
// First column element in "matrix"
|
||
|
|
const REAL * prcol = & matrix(0,icol);
|
||
|
|
|
||
|
|
// Compute the new element
|
||
|
|
REAL r = 0.0;
|
||
|
|
for (int i = 0; i < CCol(); i++)
|
||
|
|
{
|
||
|
|
assert( prcol == & matrix(i,icol) );
|
||
|
|
r += *prcol * *prrowt++;
|
||
|
|
prcol += icolInc;
|
||
|
|
}
|
||
|
|
// Store it
|
||
|
|
*prmat++ = r;
|
||
|
|
}
|
||
|
|
prrow = prrowt;
|
||
|
|
}
|
||
|
|
|
||
|
|
return mat;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX & VRMATRIX::operator += ( const VRMATRIX & vrmatrixAdd )
|
||
|
|
{
|
||
|
|
// Add vrmatrixAdd to this matrix
|
||
|
|
|
||
|
|
// Make sure the matrices are of the same dimension
|
||
|
|
|
||
|
|
if (! BSameDimension(vrmatrixAdd) )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"inapplicable matrix operator");
|
||
|
|
|
||
|
|
// Perform a flat add between all the elements in the matricies
|
||
|
|
|
||
|
|
int crealTotal = second._Totlen();
|
||
|
|
|
||
|
|
REAL* rgrealSelf = &self(0,0);
|
||
|
|
const REAL* rgrealMatrixAdd = &vrmatrixAdd(0,0);
|
||
|
|
|
||
|
|
for ( int ireal = 0 ; ireal < crealTotal ; ireal++ )
|
||
|
|
{
|
||
|
|
rgrealSelf[ireal] += rgrealMatrixAdd[ireal];
|
||
|
|
}
|
||
|
|
|
||
|
|
return self;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX & VRMATRIX::operator -= ( const VRMATRIX & vrmatrixMatrixSubtract )
|
||
|
|
{
|
||
|
|
// Subtract vrmatrixAdd from this matrix
|
||
|
|
|
||
|
|
// Make sure the matrices are of the same dimension
|
||
|
|
|
||
|
|
if ( ! BSameDimension( vrmatrixMatrixSubtract ) )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"inapplicable matrix operator");
|
||
|
|
|
||
|
|
// Perform a flat subtration between all the elements in the matricies
|
||
|
|
|
||
|
|
int crealTotal = second._Totlen();
|
||
|
|
|
||
|
|
REAL* rgrealSelf = &self(0,0);
|
||
|
|
const REAL* rgrealMatrixSubtract = &vrmatrixMatrixSubtract(0,0);
|
||
|
|
|
||
|
|
for ( int ireal = 0 ; ireal < crealTotal ; ireal++ )
|
||
|
|
{
|
||
|
|
rgrealSelf[ireal] -= rgrealMatrixSubtract[ireal];
|
||
|
|
}
|
||
|
|
|
||
|
|
return self;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX & VRMATRIX::operator *= ( REAL rScalar )
|
||
|
|
{
|
||
|
|
// Multiply each element in the matrix by rScalar
|
||
|
|
|
||
|
|
int crealTotal = second._Totlen();
|
||
|
|
|
||
|
|
REAL* rgrealSelf = &self(0,0);
|
||
|
|
|
||
|
|
for ( int ireal = 0 ; ireal < crealTotal ; ireal++ )
|
||
|
|
{
|
||
|
|
rgrealSelf[ireal] *= rScalar;
|
||
|
|
}
|
||
|
|
|
||
|
|
return self;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX & VRMATRIX::operator += ( REAL rScalar )
|
||
|
|
{
|
||
|
|
// Add rScalar to each element in the matrix
|
||
|
|
|
||
|
|
int crealTotal = second._Totlen();
|
||
|
|
|
||
|
|
REAL* rgrealSelf = &self(0,0);
|
||
|
|
|
||
|
|
for ( int ireal = 0 ; ireal < crealTotal ; ireal++ )
|
||
|
|
{
|
||
|
|
rgrealSelf[ireal] += rScalar;
|
||
|
|
}
|
||
|
|
|
||
|
|
return self;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX & VRMATRIX::operator -= ( REAL rScalar )
|
||
|
|
{
|
||
|
|
// Subtract rScalar from each element in the matrix
|
||
|
|
|
||
|
|
int crealTotal = second._Totlen();
|
||
|
|
|
||
|
|
REAL* rgrealSelf = &self(0,0);
|
||
|
|
|
||
|
|
for ( int ireal = 0 ; ireal < crealTotal ; ireal++ )
|
||
|
|
{
|
||
|
|
rgrealSelf[ireal] -= rScalar;
|
||
|
|
}
|
||
|
|
|
||
|
|
return self;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIX & VRMATRIX::operator /= ( REAL rScalar )
|
||
|
|
{
|
||
|
|
// Divide each element in the matrix by rScalar
|
||
|
|
|
||
|
|
int crealTotal = second._Totlen();
|
||
|
|
|
||
|
|
REAL* rgrealSelf = &self(0,0);
|
||
|
|
|
||
|
|
for ( int ireal = 0 ; ireal < crealTotal ; ireal++ )
|
||
|
|
{
|
||
|
|
rgrealSelf[ireal] /= rScalar;
|
||
|
|
}
|
||
|
|
|
||
|
|
return self;
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIXSQ :: VRMATRIXSQ ( const VLREAL & vrColumn, const VLREAL & vrRow )
|
||
|
|
{
|
||
|
|
// Constructor for square matrices that takes a column and row vector.
|
||
|
|
// The initial state of this matrix is the product of the input
|
||
|
|
// vectors.
|
||
|
|
|
||
|
|
// Make sure the vectors are of the same length
|
||
|
|
|
||
|
|
if ( vrColumn.size() != vrRow.size() )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"invalid matrix multiplication");
|
||
|
|
|
||
|
|
Init( vrColumn.size() );
|
||
|
|
REAL * prm = & self(0,0);
|
||
|
|
for ( int iRow = 0; iRow < CRow(); iRow++ )
|
||
|
|
{
|
||
|
|
for ( int iCol = 0; iCol < CCol(); iCol++ )
|
||
|
|
{
|
||
|
|
*prm++ = vrColumn[iCol] * vrRow[iRow];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIXSQ & VRMATRIXSQ::operator *= (const VRMATRIXSQ& matrix)
|
||
|
|
{
|
||
|
|
if ( matrix.CRow() != CRow()
|
||
|
|
|| matrix.CCol() != CRow() )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"invalid matrix multiplication");
|
||
|
|
|
||
|
|
// Temporary row for partial result
|
||
|
|
VLREAL vrrow;
|
||
|
|
vrrow.resize(CCol());
|
||
|
|
|
||
|
|
// Compute distance in flat array between rows
|
||
|
|
int icolInc = matrix.second.stride()[0];
|
||
|
|
|
||
|
|
REAL * prrow = & self(0,0);
|
||
|
|
const REAL * prmat = & matrix(0,0);
|
||
|
|
REAL * prtemp0 = & vrrow[0];
|
||
|
|
for (int irow = 0; irow < CRow(); irow++)
|
||
|
|
{
|
||
|
|
REAL * prtemp = prtemp0;
|
||
|
|
for ( int icol = 0; icol < matrix.CCol(); icol++ )
|
||
|
|
{
|
||
|
|
const REAL * prrowt = prrow;
|
||
|
|
assert( prrowt == & self(irow,0) );
|
||
|
|
|
||
|
|
// First column element in "matrix"
|
||
|
|
const REAL * prcol = & matrix(0,icol);
|
||
|
|
|
||
|
|
// Compute the new element
|
||
|
|
REAL r = 0.0;
|
||
|
|
for (int i = 0; i < CCol(); i++)
|
||
|
|
{
|
||
|
|
assert( prcol == & matrix(i,icol) );
|
||
|
|
r += *prcol * *prrowt++;
|
||
|
|
prcol += icolInc;
|
||
|
|
}
|
||
|
|
// Store it temporary row vector
|
||
|
|
*prtemp++ = r;
|
||
|
|
}
|
||
|
|
|
||
|
|
// Update row in self
|
||
|
|
prtemp = prtemp0;
|
||
|
|
for ( int icol2 = 0; icol2 < CCol(); icol2++ )
|
||
|
|
{
|
||
|
|
*prrow++ = *prtemp++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
return self;
|
||
|
|
}
|
||
|
|
|
||
|
|
void VRMATRIXSQ::LUDBackSub (const VRMATRIXSQ& matrix)
|
||
|
|
{
|
||
|
|
if ( ! matrix.BIsLUDecomposed() )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"matrix not in L-U decomposed form");
|
||
|
|
|
||
|
|
for (int icol = 0; icol < CCol(); icol++)
|
||
|
|
{
|
||
|
|
int irowNZ = -1;
|
||
|
|
|
||
|
|
for (int irow = 0; irow < CRow(); irow++)
|
||
|
|
{
|
||
|
|
int irowMax = matrix._vimdRow[irow];
|
||
|
|
REAL probSum = self(irowMax,icol);
|
||
|
|
|
||
|
|
self(irowMax,icol) = self(irow,icol);
|
||
|
|
|
||
|
|
if (irowNZ != -1)
|
||
|
|
{
|
||
|
|
for (int iMul = irowNZ; iMul < irow; iMul++)
|
||
|
|
probSum -= matrix(irow,iMul) * self(iMul,icol);
|
||
|
|
}
|
||
|
|
else if (probSum != 0.0)
|
||
|
|
irowNZ = irow;
|
||
|
|
|
||
|
|
self(irow,icol) = probSum;
|
||
|
|
}
|
||
|
|
|
||
|
|
for ( irow = CRow(); irow-- > 0; )
|
||
|
|
{
|
||
|
|
REAL probSum = self(irow,icol);
|
||
|
|
|
||
|
|
for (int iMul = irow + 1; iMul < CRow(); iMul++)
|
||
|
|
probSum -= matrix(irow,iMul) * self(iMul,icol);
|
||
|
|
self(irow,icol) = probSum / matrix(irow,irow);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void VRMATRIXSQ::LUDecompose( bool bUseTinyIfSingular )
|
||
|
|
{
|
||
|
|
// Perform L-U decomposition; throw exception if singular
|
||
|
|
// If "use tiny" is set, pivots at zero are replaced with
|
||
|
|
// RTINY value (1.0e-20)
|
||
|
|
|
||
|
|
|
||
|
|
// Check that this matrix is not already LU decomposed
|
||
|
|
if ( BIsLUDecomposed() )
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"matrix is already in L-U decomposed form");
|
||
|
|
|
||
|
|
if (CRow() == 0)
|
||
|
|
return; // trivial case
|
||
|
|
|
||
|
|
int cDim = CRow();
|
||
|
|
|
||
|
|
_vimdRow.resize(cDim);
|
||
|
|
|
||
|
|
VLREAL vlrealOverMax;
|
||
|
|
vlrealOverMax.resize(cDim);
|
||
|
|
|
||
|
|
_iSign = 1;
|
||
|
|
|
||
|
|
for (int iRow = 0; iRow < cDim; iRow++)
|
||
|
|
{
|
||
|
|
REAL realMax = 0.0;
|
||
|
|
|
||
|
|
for (int iCol = 0; iCol < cDim; iCol++)
|
||
|
|
{
|
||
|
|
REAL realAbs = fabs(self(iRow,iCol));
|
||
|
|
|
||
|
|
if (realAbs > realMax)
|
||
|
|
realMax = realAbs;
|
||
|
|
}
|
||
|
|
if (realMax == 0.0)
|
||
|
|
{
|
||
|
|
// Every element in the row is zero: this is a singular matrix
|
||
|
|
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"matrix is singular");
|
||
|
|
}
|
||
|
|
|
||
|
|
vlrealOverMax[iRow] = 1.0 / realMax;
|
||
|
|
}
|
||
|
|
|
||
|
|
for (int iCol = 0; iCol < cDim; iCol++)
|
||
|
|
{
|
||
|
|
for (int iRow = 0; iRow < iCol; iRow++)
|
||
|
|
{
|
||
|
|
REAL realSum = self(iRow,iCol);
|
||
|
|
|
||
|
|
for (int iMul = 0; iMul < iRow; iMul++)
|
||
|
|
realSum -= self(iRow,iMul) * self(iMul,iCol);
|
||
|
|
|
||
|
|
self(iRow,iCol) = realSum;
|
||
|
|
}
|
||
|
|
|
||
|
|
REAL realMax = 0.0;
|
||
|
|
int iRowMax = 0;
|
||
|
|
|
||
|
|
for ( iRow = iCol; iRow < cDim; iRow++)
|
||
|
|
{
|
||
|
|
REAL realSum = self(iRow,iCol);
|
||
|
|
|
||
|
|
for (int iMul = 0; iMul < iCol; iMul++)
|
||
|
|
realSum -= self(iRow,iMul) * self(iMul,iCol);
|
||
|
|
|
||
|
|
self(iRow,iCol) = realSum;
|
||
|
|
|
||
|
|
REAL realAbs = vlrealOverMax[iRow] * fabs(realSum);
|
||
|
|
|
||
|
|
if (realAbs >= realMax)
|
||
|
|
{
|
||
|
|
realMax = realAbs;
|
||
|
|
iRowMax = iRow;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
if (iRowMax != iCol)
|
||
|
|
{
|
||
|
|
// we need to interchange rows
|
||
|
|
_iSign *= -1;
|
||
|
|
vlrealOverMax[iRowMax] = vlrealOverMax[iCol];
|
||
|
|
InterchangeRows(iRowMax,iCol);
|
||
|
|
}
|
||
|
|
|
||
|
|
_vimdRow[iCol] = iRowMax;
|
||
|
|
|
||
|
|
REAL & rPivot = self(iCol,iCol);
|
||
|
|
|
||
|
|
if ( rPivot == 0.0 )
|
||
|
|
{
|
||
|
|
if ( ! bUseTinyIfSingular )
|
||
|
|
{
|
||
|
|
// This is a singular matrix: throw exceptioin
|
||
|
|
throw GMException(EC_MDVECT_MISUSE,"matrix is singular");
|
||
|
|
}
|
||
|
|
|
||
|
|
rPivot = RTINY;
|
||
|
|
}
|
||
|
|
|
||
|
|
REAL rScale = 1.0 / rPivot;
|
||
|
|
|
||
|
|
for ( iRow = iCol + 1; iRow < cDim; iRow++)
|
||
|
|
self(iRow,iCol) *= rScale;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
void VRMATRIXSQ::Invert( bool bUseTinyIfSingular )
|
||
|
|
{
|
||
|
|
// Invert; throw exception if singular. If not in L-U form,
|
||
|
|
// L-U Decomp is called.
|
||
|
|
|
||
|
|
if ( ! BIsLUDecomposed() )
|
||
|
|
{
|
||
|
|
LUDecompose( bUseTinyIfSingular );
|
||
|
|
}
|
||
|
|
|
||
|
|
VRMATRIXSQ matrixOne(CRow());
|
||
|
|
|
||
|
|
// Create the identity matrix
|
||
|
|
|
||
|
|
for (int iDim1 = 0; iDim1 < CRow(); iDim1++)
|
||
|
|
{
|
||
|
|
for (int iDim2 = 0; iDim2 < CRow(); iDim2++)
|
||
|
|
matrixOne(iDim1, iDim2) = iDim1 == iDim2 ? 1.0 : 0.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
matrixOne.LUDBackSub(self);
|
||
|
|
|
||
|
|
for ( iDim1 = 0; iDim1 < CRow(); iDim1++)
|
||
|
|
{
|
||
|
|
for (int iDim2 = 0; iDim2 < CRow(); iDim2++)
|
||
|
|
self(iDim1, iDim2) = matrixOne(iDim1, iDim2);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Clear l-u decomp values
|
||
|
|
_vimdRow.resize(0);
|
||
|
|
}
|
||
|
|
|
||
|
|
DBL VRMATRIXSQ::DblDeterminant()
|
||
|
|
{
|
||
|
|
DBL dblDet = _iSign;
|
||
|
|
|
||
|
|
if ( CRow() > 0 && ! BIsLUDecomposed() )
|
||
|
|
LUDecompose();
|
||
|
|
|
||
|
|
// Once the matrix has been LU decomposed, the determinant can be
|
||
|
|
// obtained by simply multiplying the elements of the diagonal
|
||
|
|
|
||
|
|
for (int iRow = 0; iRow < CRow(); iRow++)
|
||
|
|
{
|
||
|
|
dblDet *= self(iRow,iRow);
|
||
|
|
}
|
||
|
|
|
||
|
|
return dblDet;
|
||
|
|
}
|
||
|
|
|
||
|
|
DBL VRMATRIXSQ :: DblAddLogDiagonal() const
|
||
|
|
// Adds the log of each element in the diagonal and returns the sum.
|
||
|
|
{
|
||
|
|
DBL dblLogDiag = 0;
|
||
|
|
// bool bPositive = _iSign == 1;
|
||
|
|
bool bPositive = 1;
|
||
|
|
|
||
|
|
for (int iRow = 0; iRow < CRow(); iRow++)
|
||
|
|
{
|
||
|
|
if (self(iRow,iRow) < 0)
|
||
|
|
bPositive = !bPositive;
|
||
|
|
|
||
|
|
// Assert that the element is not zero. We should probably
|
||
|
|
// throw an exception here instead.
|
||
|
|
|
||
|
|
assert(self(iRow,iRow) != 0);
|
||
|
|
|
||
|
|
dblLogDiag += log (fabs(self(iRow,iRow)));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!bPositive)
|
||
|
|
{
|
||
|
|
// Got a negative determinant, so we can't take the log... throw
|
||
|
|
// an exception
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
return dblLogDiag;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
DBL VRMATRIXSQ :: DblLogDeterminant()
|
||
|
|
{
|
||
|
|
// Return the log of the determinant. If not in L-U form,
|
||
|
|
// L-U Decomp is called. Throws exception if negative.
|
||
|
|
|
||
|
|
if ( CRow() > 0 && ! BIsLUDecomposed() )
|
||
|
|
LUDecompose();
|
||
|
|
|
||
|
|
DBL dblLogDet = 0;
|
||
|
|
bool bPositive = _iSign == 1;
|
||
|
|
|
||
|
|
for (int iRow = 0; iRow < CRow(); iRow++)
|
||
|
|
{
|
||
|
|
if (self(iRow,iRow) < 0)
|
||
|
|
bPositive = !bPositive;
|
||
|
|
|
||
|
|
// Assert that the deterninant is not zero. We should probably
|
||
|
|
// throw an exception here instead.
|
||
|
|
|
||
|
|
assert(self(iRow,iRow) != 0);
|
||
|
|
|
||
|
|
dblLogDet += log (fabs(self(iRow,iRow)));
|
||
|
|
}
|
||
|
|
|
||
|
|
if (!bPositive)
|
||
|
|
{
|
||
|
|
// Got a negative determinant, so we can't take the log... throw
|
||
|
|
// an exception
|
||
|
|
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
return dblLogDet;
|
||
|
|
}
|
||
|
|
|
||
|
|
void VRMATRIXSQ :: GetLUDecompose( VRMATRIXSQ & vmatrixResult, bool bUseTinyIfSingular ) const
|
||
|
|
{
|
||
|
|
// Set vrmatResult to be the result of performing an L-U
|
||
|
|
// decomposition on the matrix. Will throw exception if
|
||
|
|
// the matrix is singular
|
||
|
|
// If "use tiny" is set, pivots at zero are replaced with
|
||
|
|
// RTINY value (1.0e-20)
|
||
|
|
|
||
|
|
// Copy this matrix into vmatrixResult...
|
||
|
|
vmatrixResult = self;
|
||
|
|
|
||
|
|
// .. and perform the decomposition
|
||
|
|
vmatrixResult.LUDecompose( bUseTinyIfSingular );
|
||
|
|
}
|
||
|
|
|
||
|
|
void VRMATRIXSQ :: GetInverse( VRMATRIXSQ & vmatrixResult, bool bUseTinyIfSingular ) const
|
||
|
|
{
|
||
|
|
// Set vrmatResult to the inverse of the matrix.
|
||
|
|
// Will throw an exception if the matrix is singular.
|
||
|
|
|
||
|
|
// Copy this matrix into vmatrixResult...
|
||
|
|
vmatrixResult = self;
|
||
|
|
|
||
|
|
/// ...and invert
|
||
|
|
vmatrixResult.Invert( bUseTinyIfSingular );
|
||
|
|
}
|
||
|
|
|
||
|
|
void VRMATRIXSQ :: GetDblDeterminant( DBL& dblDeterminant, VRMATRIXSQ & vmatrixResult ) const
|
||
|
|
{
|
||
|
|
// Get the determinant without modifying (LU decomposing) the matrix.
|
||
|
|
// vmatrixResult will contain the LU decomposed version of the matrix.
|
||
|
|
|
||
|
|
// Copy this matrix into vmatrixResult...
|
||
|
|
vmatrixResult = self;
|
||
|
|
dblDeterminant = vmatrixResult.DblDeterminant();
|
||
|
|
}
|
||
|
|
|
||
|
|
void VRMATRIXSQ :: GetDblLogDeterminant( DBL& dblLogDeterminant, VRMATRIXSQ & vmatrixResult ) const
|
||
|
|
{
|
||
|
|
// Get the log of determinant without modifying (LU decomposing) the matrix.
|
||
|
|
// vmatrixResult will contain the LU decomposed version of the matrix.
|
||
|
|
|
||
|
|
vmatrixResult = self;
|
||
|
|
dblLogDeterminant = vmatrixResult.DblLogDeterminant();
|
||
|
|
}
|