nmrInverse.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-27

  (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 _nmrInverse_h
#define _nmrInverse_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 nmrInverseDynamicData {

public:
    typedef vct::size_type size_type;

    enum {NB = 64};

protected:
    vctDynamicVector<CISSTNETLIB_INTEGER> PivotIndicesMemory;

    vctDynamicVector<CISSTNETLIB_DOUBLE> WorkspaceMemory;

    vctDynamicVectorRef<CISSTNETLIB_INTEGER> PivotIndicesReference;
    vctDynamicVectorRef<CISSTNETLIB_DOUBLE> WorkspaceReference;

    size_type SizeMember;
    bool StorageOrderMember;

    inline void SetDimension(size_type size, bool storageOrder)
    {
        SizeMember = size;
        StorageOrderMember = storageOrder;
    }

    inline void AllocatePivotIndicesWorkspace(bool allocatePivotIndices, bool allocateWorkspace)
    {
        const size_type maxSize1 = (SizeMember > 1) ? SizeMember : 1;

        // allocate PivotIndices
        if (allocatePivotIndices) {
            this->PivotIndicesMemory.SetSize(maxSize1);
            this->PivotIndicesReference.SetRef(this->PivotIndicesMemory);
        } else {
            this->PivotIndicesMemory.SetSize(0);
        }

        // allocate Workspace
        if (allocateWorkspace) {
            this->WorkspaceMemory.SetSize(maxSize1 * NB);
            this->WorkspaceReference.SetRef(this->WorkspaceMemory);
        } else {
            this->WorkspaceMemory.SetSize(0);
        }
    }


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


    template <class _vectorOwnerTypeWorkspace>
    inline void ThrowUnlessWorkspaceSizeIsCorrect(vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace)
        throw(std::runtime_error)
    {
        // check sizes and compacity
        const size_type maxSize1 = (SizeMember > 1) ? SizeMember : 1;
        if (maxSize1 * NB > workspace.size()) {
            cmnThrow(std::runtime_error("nmrInverseDynamicData: Size of vector workspace is incorrect."));
        }
        if (!workspace.IsCompact()) {
            cmnThrow(std::runtime_error("nmrInverseDynamicData: Vector workspace must be compact."));
        }
    }


public:

    template <class _matrixOwnerTypeA>
    static inline
    size_type PivotIndicesSize(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        return ((A.rows() > 1) ? A.rows() : 1);
    }


    template <class _matrixOwnerTypeA>
    static inline
    size_type WorkspaceSize(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        return ((A.rows() > 1) ? A.rows() : 1) * NB;
    }


#ifndef SWIG
#ifndef DOXYGEN

    class Friend {
    private:
        nmrInverseDynamicData & Data;
    public:
        Friend(nmrInverseDynamicData &data): Data(data) {
        }
        inline vctDynamicVectorRef<CISSTNETLIB_INTEGER> & PivotIndices(void) {
            return Data.PivotIndicesReference;
        }
        inline vctDynamicVectorRef<CISSTNETLIB_DOUBLE> & Workspace(void) {
            return Data.WorkspaceReference;
        }
        inline size_type Size(void) {
            return Data.SizeMember;
        }
        inline size_type StorageOrder(void) {
            return Data.StorageOrderMember;
        }
    };
    friend class Friend;
#endif // DOXYGEN
#endif // SWIG

    nmrInverseDynamicData():
        SizeMember(static_cast<size_type>(0)),
        StorageOrderMember(VCT_ROW_MAJOR)
    {
        AllocatePivotIndicesWorkspace(false, false);
    }

    nmrInverseDynamicData(size_type size, bool storageOrder)
    {
        this->Allocate(size, storageOrder);
    }

    template <class _matrixOwnerTypeA>
    nmrInverseDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        this->Allocate(A);
    }

    template <class _matrixOwnerTypeA,
              class _vectorOwnerTypePivotIndices,
              class _vectorOwnerTypeWorkspace>
    nmrInverseDynamicData(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                          vctDynamicVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices,
                          vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace)
    {
        this->SetRef(A, pivotIndices, workspace);
    }

    template <class _matrixOwnerTypeA>
    inline void Allocate(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
    {
        this->Allocate(A.rows(), A.StorageOrder());
    }

    void Allocate(size_type size, bool storageOrder)
    {
        this->SetDimension(size, storageOrder);
        this->AllocatePivotIndicesWorkspace(true, true);
    }

    template <class _matrixOwnerTypeA,
              class _vectorOwnerTypePivotIndices,
              class _vectorOwnerTypeWorkspace>
    void SetRef(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                vctDynamicVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices,
                vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace)
        throw(std::runtime_error)
    {
        this->SetDimension(A.rows(), A.StorageOrder());
        this->AllocatePivotIndicesWorkspace(false, false);
        this->ThrowUnlessPivotIndicesSizeIsCorrect(pivotIndices);
        this->PivotIndicesReference.SetRef(pivotIndices);
        this->ThrowUnlessWorkspaceSizeIsCorrect(workspace);
        this->WorkspaceReference.SetRef(workspace);

    }
};



#ifndef SWIG
template <vct::size_type _size, bool _storageOrder>
class nmrInverseFixedSizeData
{
public:
#ifndef DOXYGEN
    typedef vct::size_type size_type;
    enum {MAX_SIZE_1 = (_size > 1) ? _size : 1};
    enum {NB = 64};
    enum {LWORK = _size * NB};

#endif // DOXYGEN

    typedef vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _size, _size, _storageOrder> MatrixTypeA;
    typedef vctFixedSizeVector<CISSTNETLIB_INTEGER, MAX_SIZE_1> VectorTypePivotIndices;
    typedef vctFixedSizeVector<CISSTNETLIB_DOUBLE, LWORK> VectorTypeWorkspace;

protected:
    VectorTypePivotIndices PivotIndicesMember; 
    VectorTypeWorkspace WorkspaceMember; 
public:
#ifndef DOXYGEN

    class Friend {
    private:
        nmrInverseFixedSizeData<_size, _storageOrder> & Data;
    public:
        Friend(nmrInverseFixedSizeData<_size, _storageOrder> &data): Data(data) {
        }
        inline VectorTypePivotIndices & PivotIndices(void) {
            return Data.PivotIndicesMember;
        }
        inline VectorTypeWorkspace & Workspace(void) {
            return Data.WorkspaceMember;
        }
    };
    friend class Friend;
#endif // DOXYGEN

    nmrInverseFixedSizeData() {};
};
#endif // SWIG




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

    /* check that the matrix is square */
    if (!A.IsSquare()) {
        cmnThrow(std::runtime_error("nmrInverse: Input must be a square matrix."));
    }
    /* check sizes */
    if (dataFriend.Size() != A.rows()) {
        cmnThrow(std::runtime_error("nmrInverse: Size used for Allocate was different."));
    }
    /* check that the matrices are compact */
    if (! A.IsCompact()) {
        cmnThrow(std::runtime_error("nmrInverse: Requires a compact matrix."));
    }

    CISSTNETLIB_INTEGER size = dataFriend.Size();
    CISSTNETLIB_INTEGER lda = (size > 1) ? size : 1;
    CISSTNETLIB_INTEGER lwork = dataFriend.Workspace().size();
    /* call the LAPACK C function */
#if defined(CISSTNETLIB_VERSION_MAJOR)
#if (CISSTNETLIB_VERSION_MAJOR >= 3)
    cisstNetlib_dgetrf_(&size, &size,
                        A.Pointer(), &lda,
                        dataFriend.PivotIndices().Pointer(),
                        &info);
    cisstNetlib_dgetri_(&size,
                        A.Pointer(), &lda,
                        dataFriend.PivotIndices().Pointer(),
                        dataFriend.Workspace().Pointer(),
                        &lwork,
                        &info);
#endif
#else // no major version
    dgetrf_(&size, &size,
            A.Pointer(), &lda,
            dataFriend.PivotIndices().Pointer(),
            &info);
    dgetri_(&size,
            A.Pointer(), &lda,
            dataFriend.PivotIndices().Pointer(),
            dataFriend.Workspace().Pointer(),
            &lwork,
            &info);
#endif // CISSTNETLIB_VERSION
    return info;
}



template <class _matrixOwnerTypeA,
          class _vectorOwnerTypePivotIndices,
          class _vectorOwnerTypeWorkspace>
inline CISSTNETLIB_INTEGER nmrInverse(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A,
                            vctDynamicVectorBase<_vectorOwnerTypePivotIndices, CISSTNETLIB_INTEGER> & pivotIndices,
                            vctDynamicVectorBase<_vectorOwnerTypeWorkspace, CISSTNETLIB_DOUBLE> & workspace)
{
    nmrInverseDynamicData data(A, pivotIndices, workspace);
    return nmrInverse(A, data);
}


template <class _matrixOwnerTypeA>
inline CISSTNETLIB_INTEGER nmrInverse(vctDynamicMatrixBase<_matrixOwnerTypeA, CISSTNETLIB_DOUBLE> & A)
{
    nmrInverseDynamicData data(A);
    return nmrInverse(A, data);
}


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

template <vct::size_type _size, vct::size_type _maxSize1, vct::size_type _lWork, bool _storageOrder>
inline CISSTNETLIB_INTEGER nmrInverse(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _size, _size, _storageOrder> & A,
                            vctFixedSizeVector<CISSTNETLIB_INTEGER, _maxSize1> & pivotIndices,
                            vctFixedSizeVector<CISSTNETLIB_DOUBLE, _lWork> & workspace)
{
    const CISSTNETLIB_INTEGER maxSize1 = static_cast<CISSTNETLIB_INTEGER>(nmrInverseFixedSizeData<_size, _storageOrder>::MAX_SIZE_1);
    const CISSTNETLIB_INTEGER lWork = static_cast<CISSTNETLIB_INTEGER>(nmrInverseFixedSizeData<_size, _storageOrder>::LWORK);
    //Assert if requirement is equal to size provided!
    CMN_ASSERT(maxSize1 == static_cast<CISSTNETLIB_INTEGER>(_maxSize1));
    CMN_ASSERT(lWork <= static_cast<CISSTNETLIB_INTEGER>(_lWork));

    CISSTNETLIB_INTEGER info;
    CISSTNETLIB_INTEGER lda = _maxSize1;
    CISSTNETLIB_INTEGER size = _size;
    CISSTNETLIB_INTEGER lwork = lWork;

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


template <vct::size_type _size, bool _storageOrder>
inline CISSTNETLIB_INTEGER nmrInverse(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _size, _size, _storageOrder> & A,
                            nmrInverseFixedSizeData<_size, _storageOrder> & data)
{
    typename nmrInverseFixedSizeData<_size, _storageOrder>::Friend dataFriend(data);
    return nmrInverse(A, dataFriend.PivotIndices(), dataFriend.Workspace());
}


template <vct::size_type _size, bool _storageOrder>
inline CISSTNETLIB_INTEGER nmrInverse(vctFixedSizeMatrix<CISSTNETLIB_DOUBLE, _size, _size, _storageOrder> & A)
{
    nmrInverseFixedSizeData<_size, _storageOrder> data;
    return nmrInverse(A, data);
}

#endif // SWIG



#endif // _nmrInverse_h