矩阵LU分解

 JglErrorStatus MathMatrix::luDecompose(MathMatrix& lMatrix, MathMatrix& uMatrix,
        MathMatrix& permutation, const JglTolerance& tol) const
{
    MathMatrix work(*this);
    permutation.setUnitMatrix();

    double *scales = new double[_numRows];
    if (calculateScaleFactors(scales, tol) != JGL_OK) {
        delete[] scales;
        return JGL_INVALID_INPUT;
    }

    for (int column = 0; column < _numColumns; ++column) {
        calculateUpperEntry(column, work);

        double maxPivot = fabs(work(column, column) * *(scales + column));
        int pivotIndex = column;

        calculateLowerEntry(column, scales, work, maxPivot, pivotIndex);

        if (fabs(maxPivot) <= tol.getCalculation()) {
            delete[] scales;
            return JGL_MUST_BE_NON_ZERO;
        }

        divideByPivot(column, pivotIndex, work, permutation, scales);
    }

    delete[] scales;

    setLUMatrix(work, lMatrix, uMatrix);

    return JGL_OK;
}

void MathMatrix::setZeroMatrix()
{
    for (int i = 0; i < _numRows; ++i) {
        memset(*(this->_matrix + i), 0, sizeof(double) * _numColumns);
    }
}

void MathMatrix::setUnitMatrix()
{
    if (_numRows != _numColumns) {
        return;
    }

    for (int i = 0; i < _numRows; ++i) {
        for (int j = 0; j < _numColumns; ++j) {
            if (i == j) {
                *(*(_matrix + i) + j) = 1.0;
            } else {
                *(*(_matrix + i) + j) = 0.0;
            }
        }
    }
}

const std::string MathMatrix::toString() const
{
    std::stringstream ss;
    ss.setf(std::ios::fixed, std::ios::floatfield);
    ss.precision(6);

    ss << "/"MathMatrix/" " << _numRows << _numColumns << std::endl;
   
    for (int i = 0; i < _numRows; ++i) {
        for (int j = 0; j < _numColumns; ++j) {
            ss << *(*(_matrix + i) + j);
            if (j != _numColumns - 1) {
                ss << " ";
            }
        }

        ss << std::endl;
    }

    return ss.str();
}

//---------------------------------------------------------------------------

void MathMatrix::clearMatrix()
{
    for (int i = 0; i < _numRows; ++i) {
        delete[] *(_matrix + i);
    }

    delete[] _matrix;
}

void MathMatrix::copyMatrixFrom(const MathMatrix& source)
{
    this->_numRows = source._numRows;
    this->_numColumns = source._numColumns;

    this->_matrix = new double*[_numRows];

    for (int i = 0; i < _numRows; ++i) {
        *(_matrix + i) = new double[_numColumns];       
        memcpy(*(this->_matrix + i), *(source._matrix + i), sizeof(double) * _numColumns);
    }
}

//---------------------------------------------------------------------------
//  for luDecompose
JglErrorStatus MathMatrix::calculateScaleFactors(double *scales, const JglTolerance& tol) const
{
    for (int i = 0; i < _numRows; ++i) {
        double max = 0.0;
        for (int j = 0; j < _numColumns; ++j) {
            if (max < fabs(*(*(_matrix + i) + j))) {
                max = fabs(*(*(_matrix + i) + j));
            }
        }

        if (max <= tol.getCalculation()) {
            return JGL_INVALID_INPUT;
        }

        *(scales + i) = 1.0 / max;
    }

    return JGL_OK;
}

void MathMatrix::calculateUpperEntry(int column, MathMatrix& work) const
{
    for (int row = 0; row <= column; ++row) {
        for (int i = 0; i <= row - 1; ++i) {
            work(row, column) -= work(row, i) * work(i, column);
        }
    }
}

void MathMatrix::calculateLowerEntry(int column, const double *scales, MathMatrix& work,
        double& maxPivot, int& pivotIndex) const
{
    for (int row = column + 1; row < _numRows; ++row) {
        for (int i = 0; i <= column - 1; ++i) {
            work(row, column) -= work(row, i) * work(i, column);
        }

        if (maxPivot < fabs(work(row, column) * *(scales + row))) {
            maxPivot = fabs(work(row, column) * *(scales + row));
            pivotIndex = row;
        }
    }
}

void MathMatrix::divideByPivot(int column, int pivotIndex, MathMatrix& work,
        MathMatrix& permutation, double *scales) const
{
    if (pivotIndex != column) {
        work.swapRows(column, pivotIndex);
        permutation.swapRows(column, pivotIndex);
        std::swap(*(scales + column), *(scales + pivotIndex));
    }

    for (int row = column + 1; row < _numRows; ++row) {
        work(row, column) /= work(column, column);
    }
}

void MathMatrix::setLUMatrix(const MathMatrix& work, MathMatrix& lMatrix, MathMatrix& uMatrix) const
{
    lMatrix.setZeroMatrix();
    uMatrix.setZeroMatrix();

    for (int i = 0; i < _numRows; ++i) {
        for (int j = 0; j < _numColumns; ++j) {
            if (i == j) {
                lMatrix(i, j) = 1.0;
                uMatrix(i, j) = work(i, j);
            } else if (i < j) {
                uMatrix(i, j) = work(i, j);
            } else {
                lMatrix(i, j) = work(i, j);
            }
        }
    }
}