nmrIsOrthonormal.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-2018 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 _nmrIsOrthonormal_h
#define _nmrIsOrthonormal_h
 
 
#include <cisstVector/vctFixedSizeMatrix.h>
#include <cisstVector/vctDynamicMatrix.h>
 
 

template <class _elementType>
class nmrIsOrthonormalDynamicData {
 
public:
    typedef vct::size_type size_type;
 
protected:
    vctDynamicVector<_elementType> WorkspaceMemory;

    inline void SetDimension(size_type m)
    {
        Size = m * m;
    }

    inline void AllocateWorkspace(bool allocateWorkspace)
    {
        if (allocateWorkspace) {
            this->WorkspaceMemory.SetSize(Size);
            this->WorkspaceReference.SetRef(this->WorkspaceMemory);
        } else {
            this->WorkspaceMemory.SetSize(0);
        }
    }

    template <typename __vectorOwnerTypeWorkspace>
    inline void
    ThrowUnlessWorkspaceSizeIsCorrect(vctDynamicVectorBase<__vectorOwnerTypeWorkspace, _elementType> & inWorkspace) const
        CISST_THROW(std::runtime_error)
    {
        if (Size > inWorkspace.size()) {
            cmnThrow(std::runtime_error("nmrIsOrthonormalDynamicData: Workspace is too small."));
        }
        if (!inWorkspace.IsCompact()) {
            cmnThrow(std::runtime_error("nmrIsOrthonormalDynamicData: Workspace must be compact."));
        }
    }

    vctDynamicVectorRef<_elementType> WorkspaceReference;

    size_type Size;
 
public:

    static inline size_type WorkspaceSize(size_type m)
    {
        return m * m;
    }

    template <class __matrixOwnerTypeA>
    static inline size_type WorkspaceSize(const vctDynamicConstMatrixBase<__matrixOwnerTypeA, _elementType> & A)
    {
        return nmrIsOrthonormalDynamicData::WorkspaceSize(A.rows());
    }
 
 
#ifndef SWIG
#ifndef DOXYGEN
    class Friend {
    private:
        nmrIsOrthonormalDynamicData & Data;
    public:
        Friend(nmrIsOrthonormalDynamicData &data): Data(data) {
        }
        inline vctDynamicVectorRef<_elementType> & Workspace(void) {
            return Data.WorkspaceReference;
        }
        inline size_type Size(void) {
            return Data.Size;
        }
    };
    friend class Friend;
#endif // DOXYGEN
#endif // SWIG

    nmrIsOrthonormalDynamicData():
        Size(static_cast<size_type>(0))
    {
        AllocateWorkspace(false);
    }

    nmrIsOrthonormalDynamicData(size_type m)
    {
        this->Allocate(m);
    }

    template <class __matrixOwnerTypeA>
    nmrIsOrthonormalDynamicData(const vctDynamicConstMatrixBase<__matrixOwnerTypeA, _elementType> & A)
    {
        this->Allocate(A);
    }

    template <class __matrixOwnerTypeA, class __vectorOwnerTypeWorkspace>
    nmrIsOrthonormalDynamicData(const vctDynamicConstMatrixBase<__matrixOwnerTypeA, _elementType> & A,
                                vctDynamicVectorBase<__vectorOwnerTypeWorkspace, _elementType> & inWorkspace)
    {
        this->SetRef(A, inWorkspace);
    }
 

    template <class __matrixOwnerTypeA>
    inline void Allocate(const vctDynamicConstMatrixBase<__matrixOwnerTypeA, _elementType> & A)
    {
        this->Allocate(A.rows());
    }

    template <class __matrixOwnerTypeA, class __vectorOwnerTypeWorkspace>
    inline void SetRef(const vctDynamicConstMatrixBase<__matrixOwnerTypeA, _elementType> & A,
                       vctDynamicVectorBase<__vectorOwnerTypeWorkspace, _elementType> & inWorkspace)
    {
        this->SetDimension(A.rows());
 
        // allocate output and set references
        this->AllocateWorkspace(false);
 
        // set reference on user provided workspace
        this->ThrowUnlessWorkspaceSizeIsCorrect(inWorkspace);
        this->WorkspaceReference.SetRef(inWorkspace);
    }

    void Allocate(size_type m)
    {
        this->SetDimension(m);
        this->AllocateWorkspace(true);
    }
};
 
 

#ifndef SWIG
template <class _elementType, vct::size_type _rows>
class nmrIsOrthonormalFixedSizeData
{
public:
#ifndef DOXYGEN
    enum {SIZE = _rows * _rows};
#endif // DOXYGEN
    typedef vctFixedSizeVector<_elementType, SIZE> VectorTypeWorkspace;
 
protected:
    VectorTypeWorkspace WorkspaceMember; 
 
public:
#ifndef DOXYGEN
    class Friend {
    private:
        nmrIsOrthonormalFixedSizeData<_elementType, _rows> & Data;
    public:
        Friend(nmrIsOrthonormalFixedSizeData<_elementType, _rows> &data): Data(data) {
        }
        inline VectorTypeWorkspace & Workspace(void) {
            return Data.WorkspaceMember;
        }
    };
    friend class Friend;
#endif // DOXYGEN

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


template <vct::size_type _size, vct::stride_type _rowStride, vct::stride_type _colStride, typename _elementType, class _dataPtrType>
bool nmrIsOrthonormal(const vctFixedSizeConstMatrixBase<_size, _size, _rowStride, _colStride, _elementType, _dataPtrType> & A,
                      nmrIsOrthonormalFixedSizeData<_elementType, _size> & data,
                      _elementType tolerance = cmnTypeTraits<_elementType>::Tolerance()) {
    typename nmrIsOrthonormalFixedSizeData<_elementType, _size>::Friend dataFriend(data);
    vctFixedSizeMatrixRef<_elementType, _size, _size, _size, 1> product(dataFriend.Workspace().Pointer());
    product.ProductOf(A, A.TransposeRef());
    product.Diagonal().Subtract(_elementType(1));
    return (product.LinfNorm() <= tolerance);
}

template <vct::size_type _size, vct::stride_type _rowStride, vct::stride_type _colStride, typename _elementType, class _dataPtrType>
bool nmrIsOrthonormal(const vctFixedSizeConstMatrixBase<_size, _size, _rowStride, _colStride, _elementType, _dataPtrType> & A,
                      _elementType tolerance = cmnTypeTraits<_elementType>::Tolerance()) {
    typedef vctFixedSizeConstMatrixBase<_size, _size, _rowStride, _colStride, _elementType, _dataPtrType> InputType;
    typedef typename InputType::MatrixValueType MatrixValueType;
    MatrixValueType product;
    product.ProductOf(A, A.TransposeRef());
    product.Diagonal().Subtract(_elementType(1));
    return (product.LinfNorm() <= tolerance);
}

template <class _matrixOwnerTypeA, class _elementType>
bool nmrIsOrthonormal(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, _elementType> & A,
                      nmrIsOrthonormalDynamicData<_elementType> & data,
                      _elementType tolerance = cmnTypeTraits<_elementType>::Tolerance())
    CISST_THROW(std::runtime_error)
{
    typename nmrIsOrthonormalDynamicData<_elementType>::Friend dataFriend(data);
 
    /* check that this is a square matrix */
    if (A.cols() != A.rows()) {
        cmnThrow(std::runtime_error("nmrIsOrthonormal: Requires a square matrix."));
    }
    /* check that the workspace is large enough */
    if ((A.cols() * A.rows()) > dataFriend.Size()) {
        cmnThrow(std::runtime_error("nmrIsOrthonormal: The data provided is not large enough."));
    }
    vctDynamicMatrixRef<_elementType> product;
    product.SetRef(A.rows(), A.cols(), A.cols(), 1, dataFriend.Workspace().Pointer());
    product.ProductOf(A, A.TransposeRef());
    product.Diagonal().Subtract(_elementType(1));
    return (product.LinfNorm() <= tolerance);
}

template <class _matrixOwnerTypeA, typename _elementType>
bool nmrIsOrthonormal(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, _elementType> & A,
                      _elementType tolerance = cmnTypeTraits<_elementType>::Tolerance())
    CISST_THROW(std::runtime_error)
{
    nmrIsOrthonormalDynamicData<_elementType> data;
    data.Allocate(A);
    return nmrIsOrthonormal(A, data, tolerance);
}

template <class _matrixOwnerTypeA, class _vectorOwnerTypeWorkspace, typename _elementType>
bool nmrIsOrthonormal(const vctDynamicConstMatrixBase<_matrixOwnerTypeA, _elementType> & A,
                      vctDynamicVectorBase<_vectorOwnerTypeWorkspace, _elementType> & workspace,
                      _elementType tolerance = cmnTypeTraits<_elementType>::Tolerance())
    CISST_THROW(std::runtime_error)
{
    nmrIsOrthonormalDynamicData<_elementType> data;
    data.SetRef(A, workspace);
    return nmrIsOrthonormal(A, data, tolerance);
}

 
#endif // _nmrIsOrthonormal_h