Source code of Windows XP (NT5)
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

760 lines
16 KiB

//+-------------------------------------------------------------------------
//
// 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();
}