175 lines
5.4 KiB
C++
175 lines
5.4 KiB
C++
/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */
|
|
/*************************************************************************
|
|
*
|
|
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
|
|
*
|
|
* Copyright 2000, 2010 Oracle and/or its affiliates.
|
|
*
|
|
* OpenOffice.org - a multi-platform office productivity suite
|
|
*
|
|
* This file is part of OpenOffice.org.
|
|
*
|
|
* OpenOffice.org is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU Lesser General Public License version 3
|
|
* only, as published by the Free Software Foundation.
|
|
*
|
|
* OpenOffice.org 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 version 3 for more details
|
|
* (a copy is included in the LICENSE file that accompanied this code).
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public License
|
|
* version 3 along with OpenOffice.org. If not, see
|
|
* <http://www.openoffice.org/license.html>
|
|
* for a copy of the LGPLv3 License.
|
|
*
|
|
************************************************************************/
|
|
|
|
/** This method eliminates elements below main diagonal in the given
|
|
matrix by gaussian elimination.
|
|
|
|
@param matrix
|
|
The matrix to operate on. Last column is the result vector (right
|
|
hand side of the linear equation). After successful termination,
|
|
the matrix is upper triangular. The matrix is expected to be in
|
|
row major order.
|
|
|
|
@param rows
|
|
Number of rows in matrix
|
|
|
|
@param cols
|
|
Number of columns in matrix
|
|
|
|
@param minPivot
|
|
If the pivot element gets lesser than minPivot, this method fails,
|
|
otherwise, elimination succeeds and true is returned.
|
|
|
|
@return true, if elimination succeeded.
|
|
*/
|
|
template <class Matrix, typename BaseType>
|
|
bool eliminate( Matrix& matrix,
|
|
int rows,
|
|
int cols,
|
|
const BaseType& minPivot )
|
|
{
|
|
BaseType temp;
|
|
int max, i, j, k; /* *must* be signed, when looping like: j>=0 ! */
|
|
|
|
/* eliminate below main diagonal */
|
|
for(i=0; i<cols-1; ++i)
|
|
{
|
|
/* find best pivot */
|
|
max = i;
|
|
for(j=i+1; j<rows; ++j)
|
|
if( fabs(matrix[ j*cols + i ]) > fabs(matrix[ max*cols + i ]) )
|
|
max = j;
|
|
|
|
/* check pivot value */
|
|
if( fabs(matrix[ max*cols + i ]) < minPivot )
|
|
return false; /* pivot too small! */
|
|
|
|
/* interchange rows 'max' and 'i' */
|
|
for(k=0; k<cols; ++k)
|
|
{
|
|
temp = matrix[ i*cols + k ];
|
|
matrix[ i*cols + k ] = matrix[ max*cols + k ];
|
|
matrix[ max*cols + k ] = temp;
|
|
}
|
|
|
|
/* eliminate column */
|
|
for(j=i+1; j<rows; ++j)
|
|
for(k=cols-1; k>=i; --k)
|
|
matrix[ j*cols + k ] -= matrix[ i*cols + k ] *
|
|
matrix[ j*cols + i ] / matrix[ i*cols + i ];
|
|
}
|
|
|
|
/* everything went well */
|
|
return true;
|
|
}
|
|
|
|
|
|
/** Retrieve solution vector of linear system by substituting backwards.
|
|
|
|
This operation _relies_ on the previous successful
|
|
application of eliminate()!
|
|
|
|
@param matrix
|
|
Matrix in upper diagonal form, as e.g. generated by eliminate()
|
|
|
|
@param rows
|
|
Number of rows in matrix
|
|
|
|
@param cols
|
|
Number of columns in matrix
|
|
|
|
@param result
|
|
Result vector. Given matrix must have space for one column (rows entries).
|
|
|
|
@return true, if back substitution was possible (i.e. no division
|
|
by zero occurred).
|
|
*/
|
|
template <class Matrix, class Vector, typename BaseType>
|
|
bool substitute( const Matrix& matrix,
|
|
int rows,
|
|
int cols,
|
|
Vector& result )
|
|
{
|
|
BaseType temp;
|
|
int j,k; /* *must* be signed, when looping like: j>=0 ! */
|
|
|
|
/* substitute backwards */
|
|
for(j=rows-1; j>=0; --j)
|
|
{
|
|
temp = 0.0;
|
|
for(k=j+1; k<cols-1; ++k)
|
|
temp += matrix[ j*cols + k ] * result[k];
|
|
|
|
if( matrix[ j*cols + j ] == 0.0 )
|
|
return false; /* imminent division by zero! */
|
|
|
|
result[j] = (matrix[ j*cols + cols-1 ] - temp) / matrix[ j*cols + j ];
|
|
}
|
|
|
|
/* everything went well */
|
|
return true;
|
|
}
|
|
|
|
|
|
/** This method determines solution of given linear system, if any
|
|
|
|
This is a wrapper for eliminate and substitute, given matrix must
|
|
contain right side of equation as the last column.
|
|
|
|
@param matrix
|
|
The matrix to operate on. Last column is the result vector (right
|
|
hand side of the linear equation). After successful termination,
|
|
the matrix is upper triangular. The matrix is expected to be in
|
|
row major order.
|
|
|
|
@param rows
|
|
Number of rows in matrix
|
|
|
|
@param cols
|
|
Number of columns in matrix
|
|
|
|
@param minPivot
|
|
If the pivot element gets lesser than minPivot, this method fails,
|
|
otherwise, elimination succeeds and true is returned.
|
|
|
|
@return true, if elimination succeeded.
|
|
*/
|
|
template <class Matrix, class Vector, typename BaseType>
|
|
bool solve( Matrix& matrix,
|
|
int rows,
|
|
int cols,
|
|
Vector& result,
|
|
BaseType minPivot )
|
|
{
|
|
if( eliminate<Matrix,BaseType>(matrix, rows, cols, minPivot) )
|
|
return substitute<Matrix,Vector,BaseType>(matrix, rows, cols, result);
|
|
|
|
return false;
|
|
}
|
|
|
|
/* vim:set shiftwidth=4 softtabstop=4 expandtab: */
|