nmrLUSolver.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: 2005-07-27

  (C) Copyright 2005-2007 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 _nmrLUSolver_h
#define _nmrLUSolver_h


#include <cisstVector/vctFixedSizeVector.h>
#include <cisstVector/vctFixedSizeMatrix.h>
#include <cisstVector/vctDynamicVector.h>
#include <cisstVector/vctDynamicMatrix.h>

#include <cisstNumerical/nmrNetlib.h>

class nmrLUSolver {
    // we have this class so that we reserve memory only one
    // would help if svd of a same size matrix (or a matrix)
    // that doesnt change much is desired.

protected:
    CISSTNETLIB_INTEGER M;
    CISSTNETLIB_INTEGER N;
    CISSTNETLIB_INTEGER Lda;
    CISSTNETLIB_INTEGER MinMN, MaxMN;
    // vctDynamicMatrix<CISSTNETLIB_INTEGER> Ipiv;
    vctDynamicVector<CISSTNETLIB_INTEGER> Ipiv;
    CISSTNETLIB_INTEGER Info;
    bool AllocatePFlag, AllocateLUFlag;
    vctDynamicMatrix<CISSTNETLIB_DOUBLE> P, L, U;

    void AllocateP(void) {
        if (M > N) {
            P.SetSize(MaxMN, MaxMN, VCT_COL_MAJOR);
        } else {
            P.SetSize(MinMN, MinMN, VCT_COL_MAJOR);
        }
    }

    void AllocateLU(void) {
        L.SetSize(M, MinMN, VCT_COL_MAJOR);
        U.SetSize(MinMN, N, VCT_COL_MAJOR);
        // These wont change, assign now
        L.SetAll(0.0);
        L.Diagonal().SetAll(1.0);
        U.SetAll(0.0);
    }

    template <class _matrixOwnerType>
    void UpdateLU(const vctDynamicConstMatrixBase<_matrixOwnerType, CISSTNETLIB_DOUBLE>& A) {
        const unsigned int rows = A.rows();
        const unsigned int cols = A.cols();
        unsigned int rowIndex, colIndex;
        for (rowIndex = 0; rowIndex < rows; ++rowIndex) {
            for (colIndex = 0; colIndex < cols; ++colIndex) {
                if (rowIndex > colIndex) {
                    L.Element(rowIndex, colIndex) = A[rowIndex][colIndex];
                } else {
                    U.Element(rowIndex, colIndex) = A[rowIndex][colIndex];
                }
            }
        }
    }
    
    void UpdateP(void) {
        P.SetAll(0.0);
        P.Diagonal().SetAll(1.0);
        CISSTNETLIB_INTEGER rowIndex, colIndex;
        for (rowIndex = 0; rowIndex < MinMN; ++rowIndex) {
            colIndex = Ipiv[rowIndex] - 1;
            P.ExchangeColumns(rowIndex, colIndex);
        }
    }
    
    
public:
    nmrLUSolver(void):
        M(0),
        N(0),
        AllocatePFlag(false),
        AllocateLUFlag(false)
    {
        Allocate(M, N, AllocateLUFlag, AllocatePFlag);
    }


    nmrLUSolver(CISSTNETLIB_INTEGER m, CISSTNETLIB_INTEGER n,
                bool allocateLU = false,
                bool allocateP = false) {
        Allocate(m, n);
    }

    
    nmrLUSolver(const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &A,
                bool allocateLU = false, bool allocateP = false) {
        Allocate(A, allocateLU, allocateP);
    }

    template <vct::size_type _rows, vct::size_type _cols>
    nmrLUSolver(const vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, VCT_COL_MAJOR> & A,
                bool allocateLU = false, bool allocateP = false) {
        Allocate(A, allocateLU, allocateP);
    }


    inline void Allocate(CISSTNETLIB_INTEGER m, CISSTNETLIB_INTEGER n,
                         bool allocateLU = false, bool allocateP = false) {
        const CISSTNETLIB_INTEGER one = 1;
        M = m;
        N = n;
        Lda = std::max(one, M);
        MinMN = std::min(M, N);
        MaxMN = std::max(M, N);
        Ipiv.SetSize(MinMN);
        
        AllocateLUFlag = allocateLU;
        AllocatePFlag = allocateP;
        if (AllocateLUFlag) AllocateLU();
        if (AllocatePFlag) AllocateP();
    }


    inline void Allocate(const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &A,
                         bool allocateLU = false, bool allocateP = false) {
        Allocate(A.rows(), A.cols(), allocateLU, allocateP);
    }

    template <vct::size_type _rows, vct::size_type _cols>
    inline void Allocate(const vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, VCT_COL_MAJOR> & A,
                         bool allocateLU = false, bool allocateP = false) {
        Allocate(_rows, _cols, allocateLU, allocateP);
    }


    template <class _matrixOwnerType>
    inline void Solve(vctDynamicMatrixBase<_matrixOwnerType, CISSTNETLIB_DOUBLE> &A) throw (std::runtime_error) {
        /* check that the size and storage order matches with Allocate() */
        if (! A.IsColMajor()) {
            cmnThrow(std::runtime_error("nmrLUSolver Solve: The input must be column major"));
        }

        /* check sizes */
        if ((M != static_cast<CISSTNETLIB_INTEGER>(A.rows())) || (N != static_cast<CISSTNETLIB_INTEGER>(A.cols()))) {
            cmnThrow(std::runtime_error("nmrLUSolver Solve: Size used for Allocate was different"));
        }

        /* check that the matrix is compact */
        if (! A.IsCompact()) {
            cmnThrow(std::runtime_error("nmrLUSolver Solve: Requires a compact matrix"));
        }

        /* call the LAPACK C function */
        dgetrf_(&M, &N,
                A.Pointer(), &Lda,
                Ipiv.Pointer(), &Info);

        /* Fill P, LU if required */
        if (AllocateLUFlag) UpdateLU(A);
        if (AllocatePFlag) UpdateP();
    }
    
    template <vct::size_type _rows, vct::size_type _cols>
    inline void Solve(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _rows, _cols, VCT_COL_MAJOR> & A) {
        vctDynamicMatrixRef<CISSTNETLIB_DOUBLE> Aref(A);
        Solve(Aref);
    }


    inline const vctDynamicVector<CISSTNETLIB_INTEGER> &GetIpiv(void) const {
        return Ipiv;
    }

    inline CISSTNETLIB_INTEGER GetInfo(void) const {
        return Info;
    }

    inline const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &GetP(void) const throw(std::runtime_error) {
        if (!AllocatePFlag) {
            cmnThrow(std::runtime_error("nmrLUSolver GetP: P was not calculated since allocateP is not set"));
        }
        return P;
    }

    inline const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &GetL(void) const throw(std::runtime_error) {
        if (!AllocateLUFlag) {
            cmnThrow(std::runtime_error("nmrLUSolver GetL: L was not calculated since allocateLU is not set"));
        }
        return L;
    }

    inline const vctDynamicMatrix<CISSTNETLIB_DOUBLE> &GetU(void) const throw(std::runtime_error) {
        if (!AllocateLUFlag) {
            cmnThrow(std::runtime_error("nmrLUSolver GetU: U was not calculated since allocateLU is not set"));
        }
        return U;
    }
};


#ifdef CISST_COMPILER_IS_MSVC
class CISST_DEPRECATED nmrLUSolver;
#endif // CISST_COMPILER_IS_MSVC


#endif // _nmrLUSolver_h