nmrLU.h

Go to the documentation of this file.

/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */

/*

  Author(s): Anton Deguet
  Created on: 2006-01-10

  (C) Copyright 2006-2013 Johns Hopkins University (JHU), All Rights
  Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/


#ifndef _nmrLU_h
#define _nmrLU_h

#include <cisstCommon/cmnThrow.h>
#include <cisstVector/vctFixedSizeMatrix.h>
#include <cisstVector/vctDynamicMatrix.h>
#include <cisstNumerical/nmrNetlib.h>

// Always include last
#include <cisstNumerical/nmrExport.h>


class nmrLUDynamicData {

public:
    typedef vct::size_type size_type;

    typedef vctFixedSizeVector<size_type, 2> nsize_type;

protected:
    vctDynamicVector<CISSTNETLIB_INTEGER> OutputMemory;

    vctDynamicVectorRef<CISSTNETLIB_INTEGER> PivotIndicesReference;

    size_type MMember;
    size_type NMember;

    inline void SetDimension(size_type m, size_type n)
    {
        MMember = m;
        NMember = n;
    }

    inline void AllocateOutput(bool allocateOutput)
    {
        // allocate output
        if (allocateOutput) {
            const size_type minmn = (MMember < NMember) ? MMember : NMember;
            this->OutputMemory.SetSize(minmn);
            this->PivotIndicesReference.SetRef(this->OutputMemory);
        } else {
            this->OutputMemory.SetSize(0);
        }
    }


    template <class _vectorOwnerTypePivotIndices>
    inline void ThrowUnlessOutputSizeIsCorrect(vctDynamicVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices)
        throw(std::runtime_error)
    {
         // check sizes and compacity
        const size_type minmn = (MMember < NMember) ? MMember : NMember;
        if (minmn != pivotIndices.size()) {
            cmnThrow(std::runtime_error("nmrLUDynamicData: Size of vector pivotIndices is incorrect."));
        }
        if (!pivotIndices.IsCompact()) {
            cmnThrow(std::runtime_error("nmrLUDynamicData: Vector pivotIndices must be compact."));
        }
    }


public:

    template <class _matrixOwnerTypeA>
    static inline
    nsize_type MatrixPSize(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        nsize_type matrixSize(A.rows(), A.rows());
        return matrixSize;
    }

    template <class _matrixOwnerTypeA>
    static inline
    nsize_type MatrixLSize(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        const size_type minmn = (A.rows() < A.cols()) ? A.rows() : A.cols();
        nsize_type matrixSize(A.rows(), minmn);
        return matrixSize;
    }

    template <class _matrixOwnerTypeA>
    static inline
    nsize_type MatrixUSize(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        const size_type minmn = (A.rows() < A.cols()) ? A.rows() : A.cols();
        nsize_type matrixSize(minmn, A.cols());
        return matrixSize;
    }

    template <class _matrixOwnerTypeA, class _vectorOwnerTypePivotIndices, class _matrixOwnerTypeP>
    static inline
    vctDynamicMatrixBase<_matrixOwnerTypeP, CISSTNETLIB_DOUBLE> &
    UpdateMatrixP(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                  const vctDynamicConstVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices,
                  vctDynamicMatrixBase<_matrixOwnerTypeP, CISSTNETLIB_DOUBLE> & P)
        throw(std::runtime_error)
    {
        const size_type minmn = (A.rows() < A.cols()) ? A.rows() : A.cols();
        // check sizes 
        if (pivotIndices.size() != minmn) {
            cmnThrow(std::runtime_error("nmrLUDynamicData::UpdateMatrixP: Size of vector pivotIndices is incorrect."));
        }
        if (! P.IsSquare(A.rows())) {
            cmnThrow(std::runtime_error("nmrLUDynamicData::UpdateMatrixP: Size of matrix P is incorrect."));
        }
        // update permutation matrix
        P.SetAll(0.0);
        P.Diagonal().SetAll(1.0);
        size_type rowIndex, colIndex;
        for (rowIndex = 0; rowIndex < minmn; ++rowIndex) {
            colIndex = pivotIndices[rowIndex] - 1;
            P.ExchangeColumns(rowIndex, colIndex);
        }
        return P;
    }

    template <class _matrixOwnerTypeA, class _matrixOwnerTypeL, class _matrixOwnerTypeU>
    static inline 
    void UpdateMatrixLU(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                        vctDynamicMatrixBase<_matrixOwnerTypeL, CISSTNETLIB_DOUBLE> & L,
                        vctDynamicMatrixBase<_matrixOwnerTypeU, CISSTNETLIB_DOUBLE> & U)
        throw(std::runtime_error)
    {
        const size_type rows = A.rows();
        const size_type cols = A.cols();
        size_type rowIndex, colIndex;
        L.SetAll(0.0);
        L.Diagonal().SetAll(1.0);
        U.SetAll(0.0);
        for (rowIndex = 0; rowIndex < rows; ++rowIndex) {
            for (colIndex = 0; colIndex < cols; ++colIndex) {
                if (rowIndex > colIndex) {
                    L.Element(rowIndex, colIndex) = A.Element(rowIndex, colIndex);
                } else {
                    U.Element(rowIndex, colIndex) = A.Element(rowIndex, colIndex);
                }
            }
        }
 
    }


#ifndef SWIG
#ifndef DOXYGEN

    class Friend {
    private:
        nmrLUDynamicData & Data;
    public:
        Friend(nmrLUDynamicData &data): Data(data) {
        }
        inline vctDynamicVectorRef<CISSTNETLIB_INTEGER> & PivotIndices(void) {
            return Data.PivotIndicesReference;
        }
        inline size_type M(void) {
            return Data.MMember;
        }
        inline size_type N(void) {
            return Data.NMember;
        }
    };
    friend class Friend;
#endif // DOXYGEN
#endif // SWIG

    nmrLUDynamicData():
        MMember(static_cast<size_type>(0)),
        NMember(static_cast<size_type>(0))
    {
        AllocateOutput(false);
    }

    nmrLUDynamicData(size_type m, size_type n)
    {
        this->Allocate(m, n);
    }
    
    template <class _matrixOwnerTypeA>
    nmrLUDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        this->Allocate(A);
    }
    
    template <class _matrixOwnerTypeA,
              class _vectorOwnerTypePivotIndices>
    nmrLUDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                     vctDynamicVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices)
    {
        this->SetRef(A, pivotIndices);
    }

    template <class _matrixOwnerTypeA>
    inline void Allocate(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        this->Allocate(A.rows(), A.cols());
    }
    
    void Allocate(size_type m, size_type n)
    {
        this->SetDimension(m, n);
        this->AllocateOutput(true);
    }
    
    template <class _matrixOwnerTypeA,
              class _vectorOwnerTypePivotIndices>
    void SetRef(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                vctDynamicVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices)
        throw(std::runtime_error)
    {
        this->SetDimension(A.rows(), A.cols());
        this->AllocateOutput(false);
        this->ThrowUnlessOutputSizeIsCorrect(pivotIndices);
        this->PivotIndicesReference.SetRef(pivotIndices);
    }
    
    inline const vctDynamicVectorRef<CISSTNETLIB_INTEGER> & PivotIndices(void) const {
        return PivotIndicesReference;
    }
};



#ifndef SWIG
template <vct::size_type _rows, vct::size_type _cols>
class nmrLUFixedSizeData
{
public:
#ifndef DOXYGEN
    typedef vct::size_type size_type;
    enum {MIN_MN = (_rows < _cols) ? _rows : _cols};
#endif // DOXYGEN

    typedef vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, VCT_COL_MAJOR> MatrixTypeA;
    typedef vctFixedSizeVector<CISSTNETLIB_INTEGER, MIN_MN> VectorTypePivotIndices;
    typedef vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _rows, VCT_COL_MAJOR> MatrixTypeP;
    typedef vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, MIN_MN, VCT_COL_MAJOR> MatrixTypeL;
    typedef vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, MIN_MN, _cols, VCT_COL_MAJOR> MatrixTypeU;

protected:
    VectorTypePivotIndices PivotIndicesMember; 
public:
#ifndef DOXYGEN

    class Friend {
    private:
        nmrLUFixedSizeData<_rows, _cols> & Data;
    public:
        Friend(nmrLUFixedSizeData<_rows, _cols> &data): Data(data) {
        }
        inline VectorTypePivotIndices & PivotIndices(void) {
            return Data.PivotIndicesMember;
        }
    };
    friend class Friend;
#endif // DOXYGEN
    
    nmrLUFixedSizeData() {};
    
    inline const VectorTypePivotIndices & PivotIndices(void) const {
        return PivotIndicesMember;
    }
    

    inline static MatrixTypeP &
    UpdateMatrixP(const VectorTypePivotIndices & pivotIndices,
                  MatrixTypeP & P)
        throw(std::runtime_error)
    {
        // update permutation matrix
        P.SetAll(0.0);
        P.Diagonal().SetAll(1.0);
        size_type rowIndex, colIndex;
        for (rowIndex = 0; rowIndex < MIN_MN; ++rowIndex) {
            colIndex = pivotIndices[rowIndex] - 1;
            P.ExchangeColumns(rowIndex, colIndex);
        }
        return P;
    }


    static inline 
    void UpdateMatrixLU(const MatrixTypeA & A,
                        MatrixTypeL & L,
                        MatrixTypeU & U)
        throw(std::runtime_error)
    {
        vct::size_type rowIndex, colIndex;
        L.SetAll(0.0);
        L.Diagonal().SetAll(1.0);
        U.SetAll(0.0);
        for (rowIndex = 0; rowIndex < _rows; ++rowIndex) {
            for (colIndex = 0; colIndex < _cols; ++colIndex) {
                if (rowIndex > colIndex) {
                    L.Element(rowIndex, colIndex) = A.Element(rowIndex, colIndex);
                } else {
                    U.Element(rowIndex, colIndex) = A.Element(rowIndex, colIndex);
                }
            }
        }
    }
};
#endif // SWIG





template <class _matrixOwnerType>
inline CISSTNETLIB_INTEGER nmrLU(vctDynamicMatrixBase<_matrixOwnerType, CISSTNETLIB_DOUBLE> & A,
                       nmrLUDynamicData & data)
    throw (std::runtime_error)
{
    typename nmrLUDynamicData::Friend dataFriend(data);
    CISSTNETLIB_INTEGER info;

    /* check that storage order is VCT_COL_MAJOR */
    if (!A.IsColMajor()) {
        cmnThrow(std::runtime_error("nmrLU: Input must use VCT_COL_MAJOR storage order."));
    }
    /* check sizes */
    if ((dataFriend.M() != A.rows()) || (dataFriend.N() != A.cols())) {
        cmnThrow(std::runtime_error("nmrLU: Size used for Allocate was different."));
    }
    /* check that the matrices are compact */
    if (! A.IsCompact()) {
        cmnThrow(std::runtime_error("nmrLU: Requires a compact matrix."));
    }

    CISSTNETLIB_INTEGER m = dataFriend.M();
    CISSTNETLIB_INTEGER n = dataFriend.N();
    CISSTNETLIB_INTEGER lda = (m > 1) ? m : 1;

    /* call the LAPACK C function */
#if defined(CISSTNETLIB_VERSION_MAJOR)
#if (CISSTNETLIB_VERSION_MAJOR >= 3)
    cisstNetlib_dgetrf_(&m, &n,
                        A.Pointer(), &lda,
                        dataFriend.PivotIndices().Pointer(), &info);
#endif
#else // no major version
    dgetrf_(&m, &n,
            A.Pointer(), &lda,
            dataFriend.PivotIndices().Pointer(), &info);
#endif // CISSTNETLIB_VERSION
    return info;
}



template <class _matrixOwnerTypeA, class _vectorOwnerTypePivotIndices>
inline CISSTNETLIB_INTEGER nmrLU(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                       vctDynamicVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices)
{
    nmrLUDynamicData data(A, pivotIndices);
    return nmrLU(A, data);
}


#ifndef SWIG // don't have fixed size containers in Python

template <vct::size_type _rows, vct::size_type _cols, vct::size_type _minmn>
inline CISSTNETLIB_INTEGER nmrLU(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, VCT_COL_MAJOR> & A, 
                       vctFixedSizeVector<CISSTNETLIB_INTEGER, _minmn> & pivotIndices)
{
    const CISSTNETLIB_INTEGER minmn = static_cast<CISSTNETLIB_INTEGER>(nmrLUFixedSizeData<_rows, _cols>::MIN_MN);
    //Assert if requirement is equal to size provided!
    CMN_ASSERT(minmn == static_cast<CISSTNETLIB_INTEGER>(_minmn));

    CISSTNETLIB_INTEGER info;
    CISSTNETLIB_INTEGER lda = (_rows> 1) ? _rows : 1;
    CISSTNETLIB_INTEGER m = _rows;
    CISSTNETLIB_INTEGER n = _cols;

    /* call the LAPACK C function */
#if defined(CISSTNETLIB_VERSION_MAJOR)
#if (CISSTNETLIB_VERSION_MAJOR >= 3)
    cisstNetlib_dgetrf_(&m, &n,
                        A.Pointer(), &lda,
                        pivotIndices.Pointer(), &info);
#endif
#else // no major version
    dgetrf_(&m, &n,
            A.Pointer(), &lda,
            pivotIndices.Pointer(), &info);
#endif // CISSTNETLIB_VERSION
    return info;
}


template <vct::size_type _rows, vct::size_type _cols>
inline CISSTNETLIB_INTEGER nmrLU(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, VCT_COL_MAJOR> & A,
                       nmrLUFixedSizeData<_rows, _cols> & data)
{
    typename nmrLUFixedSizeData<_rows, _cols>::Friend dataFriend(data);
    return nmrLU(A, dataFriend.PivotIndices());
}
#endif // SWIG



#endif // _nmrLU_h