715 lines
20 KiB
C++
715 lines
20 KiB
C++
|
/*
|
||
|
* Portable Agile C++ Classes (PACC)
|
||
|
* Copyright (C) 2001-2004 by Marc Parizeau
|
||
|
* http://manitou.gel.ulaval.ca/~parizeau/PACC
|
||
|
*
|
||
|
* This library is free software; you can redistribute it and/or
|
||
|
* modify it under the terms of the GNU Lesser General Public
|
||
|
* License as published by the Free Software Foundation; either
|
||
|
* version 2.1 of the License, or (at your option) any later version.
|
||
|
*
|
||
|
* This library 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
|
||
|
* Lesser General Public License for more details.
|
||
|
*
|
||
|
* You should have received a copy of the GNU Lesser General Public
|
||
|
* License along with this library; if not, write to the Free Software
|
||
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||
|
*
|
||
|
* Contact:
|
||
|
* Laboratoire de Vision et Systemes Numeriques
|
||
|
* Departement de genie electrique et de genie informatique
|
||
|
* Universite Laval, Quebec, Canada, G1K 7P4
|
||
|
* http://vision.gel.ulaval.ca
|
||
|
*
|
||
|
*/
|
||
|
|
||
|
/*!
|
||
|
* \file PACC/Math/Matrix.cpp
|
||
|
* \brief Method definitions for class Matrix.
|
||
|
* \author Marc Parizeau and Christian Gagné, Laboratoire de vision et
|
||
|
systèmes numériques, Université Laval
|
||
|
* $Revision: 1.9.2.1 $
|
||
|
* $Date: 2007/09/10 18:24:08 $
|
||
|
*/
|
||
|
|
||
|
#include "Math/Matrix.hpp"
|
||
|
#include "Math/Vector.hpp"
|
||
|
#include "Util/StringFunc.hpp"
|
||
|
#include <stdexcept>
|
||
|
#include <iomanip>
|
||
|
#include <cmath>
|
||
|
|
||
|
using namespace std;
|
||
|
using namespace PACC;
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::add(Matrix& outMatrix, double inScalar) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "add() invalid matrix!");
|
||
|
outMatrix.setRowsCols(mRows, mCols);
|
||
|
for(unsigned int i = 0; i < size(); ++i) outMatrix[i] = (*this)[i] + inScalar;
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::add(Matrix& outMatrix, const Matrix& inMatrix) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "add() invalid matrix!");
|
||
|
PACC_AssertM(mRows == inMatrix.mRows && mCols == inMatrix.mCols, "add() matrix mismatch!");
|
||
|
outMatrix.setRowsCols(mRows, mCols);
|
||
|
for(unsigned int i = 0; i < size(); ++i) outMatrix[i] = (*this)[i] + inMatrix[i];
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
void Matrix::computeBackSubLU(const vector<unsigned int>& inIndexes, Matrix& ioMatrix) const
|
||
|
{
|
||
|
unsigned int lII = UINT_MAX;
|
||
|
for(unsigned int i = 0; i < mRows; ++i) {
|
||
|
double lSum = ioMatrix(inIndexes[i], 0);
|
||
|
ioMatrix(inIndexes[i], 0) = ioMatrix(i, 0);
|
||
|
if(lII != UINT_MAX) {
|
||
|
for(unsigned int j = lII; j < i; ++j) lSum -= (*this)(i,j) * ioMatrix(j, 0);
|
||
|
} else if(lSum != 0.0) lII = i;
|
||
|
ioMatrix(i, 0) = lSum;
|
||
|
}
|
||
|
for(unsigned int i = 0; i < mRows; ++i) {
|
||
|
const unsigned int lR = mRows-i-1;
|
||
|
double lSum = ioMatrix(lR,0);
|
||
|
for(unsigned int j = lR+1; j < mCols; ++j) lSum -= (*this)(lR,j) * ioMatrix(j,0);
|
||
|
ioMatrix(lR,0) = lSum / (*this)(lR,lR);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
double Matrix::computeDeterminant(void) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "computeDeterminant() invalid matrix!");
|
||
|
PACC_AssertM(mRows == mCols, "computeDeterminant() matrix not square!");
|
||
|
Matrix lTmp = *this;
|
||
|
vector<unsigned int> lIndexes(mRows);
|
||
|
int lD;
|
||
|
lTmp.decomposeLU(lIndexes, lD);
|
||
|
double lResult = lD;
|
||
|
for(unsigned int i = 0; i < mRows; ++i) lResult *= lTmp(i,i);
|
||
|
return lResult;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
void Matrix::computeEigens(Vector& outValues, Matrix& outVectors) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "computeEigens() invalid matrix!");
|
||
|
PACC_AssertM(mRows == mCols, "computeEigens() matrix not square!");
|
||
|
outValues.resize(mRows);
|
||
|
outVectors.resize(mRows, mCols);
|
||
|
|
||
|
// Computer eigenvectors/eigenvalues using Triagonal QL method
|
||
|
Vector lE(mRows);
|
||
|
tred2(outValues, lE, outVectors);
|
||
|
tql2(outValues, lE, outVectors);
|
||
|
|
||
|
// Sort by eigenvalues.
|
||
|
for(unsigned int j = 0; j < outValues.size(); ++j) {
|
||
|
double lMax=outValues[j];
|
||
|
unsigned int lMaxArg=j;
|
||
|
for(unsigned int l = j+1; l<outValues.size(); ++l) {
|
||
|
if(outValues[l] > lMax) {
|
||
|
lMax=outValues[l];
|
||
|
lMaxArg=l;
|
||
|
}
|
||
|
}
|
||
|
if(lMaxArg != j) {
|
||
|
for(unsigned int r = 0; r < outVectors.mRows; ++r) {
|
||
|
double lTmp = outVectors(r,j);
|
||
|
outVectors(r,j) = outVectors(r,lMaxArg);
|
||
|
outVectors(r,lMaxArg) = lTmp;
|
||
|
}
|
||
|
double lTmp = outValues[j];
|
||
|
outValues[j] = outValues[lMaxArg];
|
||
|
outValues[lMaxArg] = lTmp;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
void Matrix::decomposeLU(vector<unsigned int>& outIndexes, int& outD)
|
||
|
{
|
||
|
outD = 1;
|
||
|
vector<double> lScales;
|
||
|
scaleLU(lScales);
|
||
|
for(unsigned int j = 0; j < mCols; ++j) {
|
||
|
for(unsigned int i = 0; i < j; ++i) {
|
||
|
double lSum = (*this)(i, j);
|
||
|
for(unsigned int k = 0; k < i; ++k) lSum -= (*this)(i,k) * (*this)(k,j);
|
||
|
(*this)(i, j) = lSum;
|
||
|
}
|
||
|
double lMax = 0;
|
||
|
unsigned int l = j;
|
||
|
for(unsigned int i = j; i < mRows; ++i) {
|
||
|
double lSum = (*this)(i,j);
|
||
|
for(unsigned int k = 0; k < j; ++k) lSum -= (*this)(i,k) * (*this)(k,j);
|
||
|
(*this)(i, j) = lSum;
|
||
|
double lTmp = lScales[i] * fabs(lSum);
|
||
|
if(lTmp >= lMax) {
|
||
|
l = i;
|
||
|
lMax = lTmp;
|
||
|
}
|
||
|
}
|
||
|
if(j != l) {
|
||
|
for(unsigned int k = 0; k < (*this).mCols; ++k) {
|
||
|
double lTmp = (*this)(l,k);
|
||
|
(*this)(l,k) = (*this)(j,k);
|
||
|
(*this)(j,k) = lTmp;
|
||
|
}
|
||
|
outD = -outD;
|
||
|
lScales[l] = lScales[j];
|
||
|
}
|
||
|
outIndexes[j] = l;
|
||
|
if((*this)(j,j) == 0.0) (*this)(j,j) = 1e-20;
|
||
|
if(j != (mCols-1)) {
|
||
|
double lDummy = 1.0 / (*this)(j,j);
|
||
|
for(unsigned int i = j+1; i < mRows; ++i) (*this)(i,j) *= lDummy;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::extract(Matrix& outMatrix, unsigned int inRow1, unsigned int inRow2, unsigned int inCol1, unsigned int inCol2) const
|
||
|
{
|
||
|
PACC_AssertM(inRow1 <= inRow2 && inCol1 <= inCol2 && inRow2 < mRows && inCol2 < mCols, "extract() invalid indexes!");
|
||
|
if(&outMatrix != this) {
|
||
|
// output matrix is not self assigning
|
||
|
outMatrix.setRowsCols(inRow2-inRow1+1, inCol2-inCol1+1);
|
||
|
for(unsigned int i = inRow1; i <= inRow2; ++i) {
|
||
|
for(unsigned int j = inCol1; j <= inCol2; ++j) {
|
||
|
outMatrix(i-inRow1,j-inCol1) = (*this)(i,j);
|
||
|
}
|
||
|
}
|
||
|
} else {
|
||
|
// use temporary matrix to self assign
|
||
|
Matrix lMatrix(*this);
|
||
|
outMatrix.setRowsCols(inRow2-inRow1+1, inCol2-inCol1+1);
|
||
|
for(unsigned int i = inRow1; i <= inRow2; ++i) {
|
||
|
for(unsigned int j = inCol1; j <= inCol2; ++j) {
|
||
|
outMatrix(i-inRow1,j-inCol1) = lMatrix(i,j);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::extractColumn(Matrix& outMatrix, unsigned int inCol) const
|
||
|
{
|
||
|
return extract(outMatrix, 0, mRows-1, inCol, inCol);
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::extractRow(Matrix& outMatrix, unsigned int inRow) const
|
||
|
{
|
||
|
return extract(outMatrix, inRow, inRow, 0, mCols-1);
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
double Matrix::hypot(double a, double b) const
|
||
|
{
|
||
|
double r;
|
||
|
if(abs(a) > abs(b)) {
|
||
|
r = b/a;
|
||
|
r = abs(a)*sqrt(1+r*r);
|
||
|
}
|
||
|
else if(b != 0) {
|
||
|
r = a/b;
|
||
|
r = abs(b)*sqrt(1+r*r);
|
||
|
}
|
||
|
else {
|
||
|
r = 0.0;
|
||
|
}
|
||
|
return r;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
Matrix Matrix::invert(void) const
|
||
|
{
|
||
|
Matrix lMatrix;
|
||
|
return invert(lMatrix);
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::invert(Matrix& outMatrix) const
|
||
|
{
|
||
|
PACC_AssertM(mRows == mCols, "invert() matrix not square!");
|
||
|
Matrix lTmp = *this;
|
||
|
vector<unsigned int> lIndexes(mRows);
|
||
|
int lD;
|
||
|
lTmp.decomposeLU(lIndexes, lD);
|
||
|
outMatrix.setIdentity(mRows);
|
||
|
Matrix lB(mRows, 1);
|
||
|
for(unsigned int j = 0; j < mCols; ++j) {
|
||
|
for(unsigned int i = 0; i < mRows; ++i) lB(i,0) = outMatrix(i,j);
|
||
|
lTmp.computeBackSubLU(lIndexes, lB);
|
||
|
for(unsigned int i = 0; i < mRows; ++i) outMatrix(i,j) = lB(i,0);
|
||
|
}
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::multiply(Matrix& outMatrix, double inScalar) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "multiply() invalid matrix!");
|
||
|
outMatrix.setRowsCols(mRows, mCols);
|
||
|
for(unsigned int i = 0; i < size(); ++i) outMatrix[i] = (*this)[i] * inScalar;
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::multiply(Matrix& outMatrix, const Matrix& inMatrix) const
|
||
|
{
|
||
|
PACC_AssertM(mCols == inMatrix.mRows, "multiply() matrix mismatch!");
|
||
|
if(&outMatrix != this && &outMatrix != &inMatrix) {
|
||
|
// output matrix is neither left or right matrix (no self assigment)
|
||
|
outMatrix.setRowsCols(mRows, inMatrix.mCols);
|
||
|
for(unsigned int i = 0; i < outMatrix.mRows; ++i) {
|
||
|
for(unsigned int j = 0; j < outMatrix.mCols; ++j) {
|
||
|
outMatrix(i,j) = 0;
|
||
|
for(unsigned int k = 0; k < mCols; ++k) {
|
||
|
outMatrix(i,j) += (*this)(i,k) * inMatrix(k,j);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
} else if(&outMatrix == this && &outMatrix != &inMatrix) {
|
||
|
// use temporary matrix to self assign with left matrix
|
||
|
Matrix lMatrix(*this);
|
||
|
outMatrix.setRowsCols(mRows, inMatrix.mCols);
|
||
|
for(unsigned int i = 0; i < outMatrix.mRows; ++i) {
|
||
|
for(unsigned int j = 0; j < outMatrix.mCols; ++j) {
|
||
|
outMatrix(i,j) = 0;
|
||
|
for(unsigned int k = 0; k < mCols; ++k) {
|
||
|
outMatrix(i,j) += lMatrix(i,k) * inMatrix(k,j);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
} else if(&outMatrix != this && &outMatrix == &inMatrix) {
|
||
|
// use temporary matrix to self assign with right matrix
|
||
|
Matrix lMatrix(inMatrix);
|
||
|
outMatrix.setRowsCols(mRows, inMatrix.mCols);
|
||
|
for(unsigned int i = 0; i < outMatrix.mRows; ++i) {
|
||
|
for(unsigned int j = 0; j < outMatrix.mCols; ++j) {
|
||
|
outMatrix(i,j) = 0;
|
||
|
for(unsigned int k = 0; k < mCols; ++k) {
|
||
|
outMatrix(i,j) += (*this)(i,k) * lMatrix(k,j);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
} else {
|
||
|
// use temporary matrix to self assign with both left and right matrices
|
||
|
Matrix lMatrix(*this);
|
||
|
outMatrix.setRowsCols(mRows, inMatrix.mCols);
|
||
|
for(unsigned int i = 0; i < outMatrix.mRows; ++i) {
|
||
|
for(unsigned int j = 0; j < outMatrix.mCols; ++j) {
|
||
|
outMatrix(i,j) = 0;
|
||
|
for(unsigned int k = 0; k < mCols; ++k) {
|
||
|
outMatrix(i,j) += lMatrix(i,k) * lMatrix(k,j);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
Matrix elements must be enumerated in row order and delimited by either commas
|
||
|
(','), semi-columns (';'), or white space. The recommended style is to seperate
|
||
|
elements with comas, and rows with semi-columns. For example:
|
||
|
\verbatim
|
||
|
<Matrix name="My Matrix" rows="3" cols="4">1,2,3,4;5,6,7,8;9,10,11,12</Matrix>
|
||
|
\endverbatim
|
||
|
The number of elements must match the product of the "rows" and "cols" attributes.
|
||
|
*/
|
||
|
string Matrix::read(const XML::Iterator& inNode)
|
||
|
{
|
||
|
if(!inNode) throw runtime_error("Matrix::read() nothing to read!");
|
||
|
clear();
|
||
|
for(XML::Iterator lChild = inNode->getFirstChild(); lChild; ++lChild) {
|
||
|
if(lChild->getType() == XML::eString) {
|
||
|
istringstream lStream(lChild->getValue());
|
||
|
Tokenizer lTokenizer(lStream);
|
||
|
lTokenizer.setDelimiters(" \n\r\t,;", "");
|
||
|
string lToken;
|
||
|
while(lTokenizer.getNextToken(lToken)) push_back(String::convertToFloat(lToken));
|
||
|
}
|
||
|
}
|
||
|
mRows = String::convertToInteger(inNode->getAttribute("rows"));
|
||
|
mCols = String::convertToInteger(inNode->getAttribute("cols"));
|
||
|
if(vector<double>::size() != mRows*mCols) {
|
||
|
throwError("Matrix::read() number of elements does not match the rows x cols attributes", inNode);
|
||
|
}
|
||
|
string lName = inNode->getAttribute("name");
|
||
|
if(lName != "") mName = lName;
|
||
|
return lName;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
void Matrix::resize(unsigned int inRows, unsigned int inCols)
|
||
|
{
|
||
|
Matrix lMat(*this);
|
||
|
setRowsCols(inRows, inCols);
|
||
|
for(unsigned int i = 0; i < mRows; ++i) {
|
||
|
for(unsigned int j = 0; j < mCols; ++j) {
|
||
|
(*this)(i,j) = (i < lMat.mRows && j < lMat.mCols ? lMat(i,j) : 0.);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
void Matrix::scaleLU(vector<double>& outScales) const
|
||
|
{
|
||
|
outScales.resize(mCols);
|
||
|
for(unsigned int i = 0; i < mRows; ++i) {
|
||
|
double lMax = 0.;
|
||
|
for(unsigned int j = 0; j < mCols; ++j) {
|
||
|
const double lTmp = fabs((*this)(i,j));
|
||
|
if(lTmp > lMax) lMax=lTmp;
|
||
|
}
|
||
|
if(lMax == 0.) throw runtime_error("<Matrix::scaleLU> matrix is singular!");
|
||
|
outScales[i] = 1./lMax;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
void Matrix::setIdentity(unsigned int inSize)
|
||
|
{
|
||
|
setRowsCols(inSize, inSize);
|
||
|
for(unsigned int j = 0; j < mCols; ++j) {
|
||
|
for(unsigned int i = 0; i < mRows; ++i) {
|
||
|
(*this)(i,j) = (i == j ? 1 : 0);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::subtract(Matrix& outMatrix, double inScalar) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "subtract() invalid matrix!");
|
||
|
outMatrix.setRowsCols(mRows, mCols);
|
||
|
for(unsigned int i = 0; i < size(); ++i) outMatrix[i] = (*this)[i] - inScalar;
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::subtract(Matrix& outMatrix, const Matrix& inMatrix) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "subtract() invalid matrix!");
|
||
|
PACC_AssertM(mRows == inMatrix.mRows && mCols == inMatrix.mCols, "subtract() matrix mismatch!");
|
||
|
outMatrix.setRowsCols(mRows, mCols);
|
||
|
for(unsigned int i = 0; i < size(); ++i) outMatrix[i] = (*this)[i] - inMatrix[i];
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
* \param d Real part of eigenvalues computed from the matrix.
|
||
|
* \param e Imaginary part of eigenvalues computed from the matrix.
|
||
|
* \param V Eigenvectors computed from the matrix.
|
||
|
*
|
||
|
* This method is derived from procedure tql2 of the Java package JAMA,
|
||
|
* which is itself derived from the Algol procedures tql2, by
|
||
|
* Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
|
||
|
* Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
|
||
|
* Fortran subroutine in EISPACK.
|
||
|
*/
|
||
|
void Matrix::tql2(Vector& d, Vector& e, Matrix& V) const
|
||
|
{
|
||
|
const unsigned int n=mRows;
|
||
|
for(unsigned int i = 1; i < n; i++) e[i-1] = e[i];
|
||
|
e[n-1] = 0.0;
|
||
|
|
||
|
double f = 0.0;
|
||
|
double tst1 = 0.0;
|
||
|
double eps = std::pow(2.0,-52.0);
|
||
|
for(unsigned int l = 0; l < n; l++) {
|
||
|
// Find small subdiagonal element
|
||
|
tst1 = max(tst1, abs(d[l]) + abs(e[l]));
|
||
|
unsigned int m=l;
|
||
|
while((m+1) < n) {
|
||
|
if(std::abs(e[m]) <= eps*tst1) break;
|
||
|
m++;
|
||
|
}
|
||
|
|
||
|
// If m == l, d[l] is an eigenvalue,
|
||
|
// otherwise, iterate.
|
||
|
if(m > l) {
|
||
|
unsigned int iter = 0;
|
||
|
do {
|
||
|
iter = iter + 1; // (Could check iteration count here.)
|
||
|
// Compute implicit shift
|
||
|
double g = d[l];
|
||
|
double p = (d[l+1] - g) / (2.0 * e[l]);
|
||
|
double r = hypot(p,1.0);
|
||
|
if(p < 0) r = -r;
|
||
|
d[l] = e[l] / (p + r);
|
||
|
d[l+1] = e[l] * (p + r);
|
||
|
double dl1 = d[l+1];
|
||
|
double h = g - d[l];
|
||
|
for(unsigned int i = l+2; i < n; i++) d[i] -= h;
|
||
|
f = f + h;
|
||
|
|
||
|
// Implicit QL transformation.
|
||
|
p = d[m];
|
||
|
double c = 1.0;
|
||
|
double c2 = c;
|
||
|
double c3 = c;
|
||
|
double el1 = e[l+1];
|
||
|
double s = 0.0;
|
||
|
double s2 = 0.0;
|
||
|
for(unsigned int i = m-1; i >= l; i--) {
|
||
|
c3 = c2;
|
||
|
c2 = c;
|
||
|
s2 = s;
|
||
|
g = c * e[i];
|
||
|
h = c * p;
|
||
|
r = hypot(p,e[i]);
|
||
|
e[i+1] = s * r;
|
||
|
s = e[i] / r;
|
||
|
c = p / r;
|
||
|
p = c * d[i] - s * g;
|
||
|
d[i+1] = h + s * (c * g + s * d[i]);
|
||
|
|
||
|
// Accumulate transformation.
|
||
|
for(unsigned int k = 0; k < n; k++) {
|
||
|
h = V(k,i+1);
|
||
|
V(k,i+1) = s * V(k,i) + c * h;
|
||
|
V(k,i) = c * V(k,i) - s * h;
|
||
|
}
|
||
|
if(i == 0) break;
|
||
|
}
|
||
|
p = -s * s2 * c3 * el1 * e[l] / dl1;
|
||
|
e[l] = s * p;
|
||
|
d[l] = c * p;
|
||
|
|
||
|
// Check for convergence.
|
||
|
} while (std::abs(e[l]) > eps*tst1);
|
||
|
}
|
||
|
d[l] = d[l] + f;
|
||
|
e[l] = 0.0;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
* \param d Real part of eigenvalues computed from the matrix.
|
||
|
* \param e Imaginary part of eigenvalues computed from the matrix.
|
||
|
* \param V Eigenvectors computed from the matrix.
|
||
|
*
|
||
|
* This method is derived from procedure tred2 of the Java package JAMA,
|
||
|
* which is itself derived from the Algol procedures tred2, by
|
||
|
* Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
|
||
|
* Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
|
||
|
* Fortran subroutine in EISPACK.
|
||
|
*/
|
||
|
void Matrix::tred2(Vector& d, Vector& e, Matrix& V) const
|
||
|
{
|
||
|
const unsigned int n=mRows;
|
||
|
V = *this;
|
||
|
for(unsigned int j = 0; j < n; ++j) d[j] = V(n-1,j);
|
||
|
|
||
|
// Householder reduction to tridiagonal form.
|
||
|
for(unsigned int i = n-1; i > 0; --i) {
|
||
|
|
||
|
// Scale to avoid under/overflow.
|
||
|
double scale = 0.0;
|
||
|
double h = 0.0;
|
||
|
for(unsigned int k = 0; k < i; ++k) scale += abs(d[k]);
|
||
|
if(scale == 0.0) {
|
||
|
e[i] = d[i-1];
|
||
|
for(unsigned int j = 0; j < i; ++j) {
|
||
|
d[j] = V(i-1,j);
|
||
|
V(i,j) = 0.0;
|
||
|
V(j,i) = 0.0;
|
||
|
}
|
||
|
} else {
|
||
|
// Generate Householder vector.
|
||
|
for(unsigned int k=0; k<i; ++k) {
|
||
|
d[k] /= scale;
|
||
|
h += d[k] * d[k];
|
||
|
}
|
||
|
double f = d[i-1];
|
||
|
double g = sqrt(h);
|
||
|
if(f > 0.0) g = -g;
|
||
|
e[i] = scale * g;
|
||
|
h = h - f * g;
|
||
|
d[i-1] = f - g;
|
||
|
for(unsigned int j = 0; j < i; j++) e[j] = 0.0;
|
||
|
|
||
|
// Apply similarity transformation to remaining columns.
|
||
|
for(unsigned int j = 0; j < i; j++) {
|
||
|
f = d[j];
|
||
|
V(j,i) = f;
|
||
|
g = e[j] + V(j,j) * f;
|
||
|
for(unsigned int k = j+1; k <= i-1; k++) {
|
||
|
g += V(k,j) * d[k];
|
||
|
e[k] += V(k,j) * f;
|
||
|
}
|
||
|
e[j] = g;
|
||
|
}
|
||
|
f = 0.0;
|
||
|
for(unsigned int j = 0; j < i; j++) {
|
||
|
e[j] /= h;
|
||
|
f += e[j] * d[j];
|
||
|
}
|
||
|
double hh = f / (h + h);
|
||
|
for(unsigned int j=0; j<i; j++) e[j] -= hh * d[j];
|
||
|
for(unsigned int j=0; j<i; j++) {
|
||
|
f = d[j];
|
||
|
g = e[j];
|
||
|
for(unsigned int k = j; k <= i-1; k++) V(k,j) -= (f * e[k] + g * d[k]);
|
||
|
d[j] = V(i-1,j);
|
||
|
V(i,j) = 0.0;
|
||
|
}
|
||
|
}
|
||
|
d[i] = h;
|
||
|
}
|
||
|
|
||
|
// Accumulate transformations.
|
||
|
for(unsigned int i = 0; i < n-1; i++) {
|
||
|
V(n-1,i) = V(i,i);
|
||
|
V(i,i) = 1.0;
|
||
|
double h = d[i+1];
|
||
|
if(h!=0.0) {
|
||
|
for(unsigned int k=0; k<=i; k++) d[k] = V(k,i+1) / h;
|
||
|
for(unsigned int j=0; j<=i; j++) {
|
||
|
double g = 0.0;
|
||
|
for(unsigned int k=0; k<=i; k++) g += V(k,i+1) * V(k,j);
|
||
|
for(unsigned int k=0; k<=i; k++) V(k,j) -= g * d[k];
|
||
|
}
|
||
|
}
|
||
|
for(unsigned int k=0; k<=i; k++) V(k,i+1) = 0.0;
|
||
|
}
|
||
|
for(unsigned int j=0; j<n; j++) {
|
||
|
d[j] = V(n-1,j);
|
||
|
V(n-1,j) = 0.0;
|
||
|
}
|
||
|
V(n-1,n-1) = 1.0;
|
||
|
e[0] = 0.0;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
Matrix Matrix::transpose(void) const
|
||
|
{
|
||
|
Matrix lMatrix;
|
||
|
return transpose(lMatrix);
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method also returns a reference to the result.
|
||
|
*/
|
||
|
Matrix& Matrix::transpose(Matrix& outMatrix) const
|
||
|
{
|
||
|
PACC_AssertM(mRows > 0 && mCols > 0, "transpose() invalid matrix!");
|
||
|
if(&outMatrix != this) {
|
||
|
// output matrix is not self assigning
|
||
|
outMatrix.setRowsCols(mCols,mRows);
|
||
|
// transpose elements
|
||
|
for(unsigned int i = 0; i< mRows; ++i) {
|
||
|
for(unsigned int j = 0; j < mCols; ++j) {
|
||
|
outMatrix(j,i) = (*this)(i,j);
|
||
|
}
|
||
|
}
|
||
|
} else {
|
||
|
// use temporary matrix to self assign
|
||
|
Matrix lMatrix(*this);
|
||
|
outMatrix.setRowsCols(mCols,mRows);
|
||
|
// transpose elements
|
||
|
for(unsigned int i = 0; i< mRows; ++i) {
|
||
|
for(unsigned int j = 0; j < mCols; ++j) {
|
||
|
outMatrix(j,i) = lMatrix(i,j);
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
return outMatrix;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
void Matrix::throwError(const string& inMessage, const XML::Iterator& inNode) const
|
||
|
{
|
||
|
ostringstream lStream;
|
||
|
lStream << inMessage << " for markup:\n";
|
||
|
XML::Streamer lStreamer(lStream);
|
||
|
inNode->serialize(lStreamer);
|
||
|
throw runtime_error(lStream.str());
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
See Matrix::read for a description of the write format. By default, the precision
|
||
|
of the output is set to 15 digits. This value can be changed using method
|
||
|
Matrix::setPrecision.
|
||
|
*/
|
||
|
void Matrix::write(XML::Streamer& outStream, const string& inTag) const
|
||
|
{
|
||
|
outStream.openTag(inTag, false);
|
||
|
if(mName != "") outStream.insertAttribute("name", mName);
|
||
|
outStream.insertAttribute("rows", mRows);
|
||
|
outStream.insertAttribute("cols", mCols);
|
||
|
ostringstream lContent;
|
||
|
lContent.precision(mPrec);
|
||
|
for(unsigned int i = 0; i < size(); ++i) {
|
||
|
if(i != 0 && i % mCols == 0) lContent << ";";
|
||
|
else if(i != 0) lContent << ",";
|
||
|
lContent << (*this)[i];
|
||
|
}
|
||
|
outStream.insertStringContent(lContent.str());
|
||
|
outStream.closeTag();
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
*/
|
||
|
ostream& PACC::operator<<(ostream &outStream, const Matrix& inMatrix)
|
||
|
{
|
||
|
XML::Streamer lStream(outStream);
|
||
|
inMatrix.write(lStream);
|
||
|
return outStream;
|
||
|
}
|
||
|
|
||
|
/*!
|
||
|
This method uses the first data tag of the parse tree to read the matrix. The
|
||
|
corresponding tree root is then erased. Any read error throws a std::runtime_error.
|
||
|
*/
|
||
|
XML::Document& PACC::operator>>(XML::Document& inDocument, Matrix& outMatrix)
|
||
|
{
|
||
|
XML::Iterator lNode = inDocument.getFirstDataTag();
|
||
|
outMatrix.read(lNode);
|
||
|
inDocument.erase(lNode);
|
||
|
return inDocument;
|
||
|
}
|