CBinaryRelation.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CBINARYRELATION_H_
#define CBINARYRELATION_H_

#include <mrpt/math/CMatrixTemplate.h>
#include <mrpt/math/matrix_adaptors.h>

#include <set>
#include <iterator>
#include <algorithm>
#include <utility>

namespace mrpt  {       namespace math  {
        using std::vector;

        /**
          * This class models a binary relation through the elements of any given set. I.e. for each pair of elements (A,B) it assigns two values, f(A,B) and f(B,A).
          * This class is useful when calling the base function is costly, since it acts like a proxy. It's also useful if the relationship values do not correspond
          * with the return value of a function. Although it theoretically supports objects with non-trivial constructors or destructors (indicated by specifying
          * "true" as the thrid parameter of the template instantiation), certain operations will cause memory leaks and may even cause undefined behaviour, so it's
          * reccomended to use only basic types for the parameter U. The parameter T may be any complex object, however, like a smart pointer.
         * \ingroup mrpt_base_grp
          */
        template<typename T,typename U,bool UIsObject=false> class CBinaryRelation      {
        private:
                //TODO: VIRTUALIZE INSERTROWSANDCOLS!!! AND REIMPLEMENT IN CMATRIXTEMPLATEOBJECTS.

                typedef typename detail::MatrixWrapper<U,UIsObject>::MatrixType MatrixType;     //!<Matrix type used to store the actual relation.
        public:
                typedef U (*SimpleFunctionByReturnValue)(T,T);  //!< Simple function type, used to initialize chunks of the matrix.
                typedef U (*FunctionByReturnValue)(const T &,const T &);        //!<Function type which obtains the relation value by a return value.
                typedef void (*FunctionByReferencePass)(const T &,const T &,U &);       //!<Function type which obtains the relation value by reference pass.
                typedef typename std::set<T>::const_iterator const_iterator;    //!<Constant iterator through the set elements.
                typedef typename std::set<T>::const_reverse_iterator const_reverse_iterator;    //!<Constant reverse iterator through the set elements.
                typedef CMatrixRowAccessor<U> AccessorForFirstElement;  //!<Accessor type to every value related to any element A, i.e., f(A,x).
                typedef CMatrixColumnAccessor<U> AccessorForSecondElement;      //!<Accessor type to every value related to any element B, i.e., f(x,B).
                typedef CConstMatrixRowAccessor<U> ConstAccessorForFirstElement;        //!<Const accessor type to every value related to any element A, i.e., f(A,x).
                typedef CConstMatrixColumnAccessor<U> ConstAccessorForSecondElement;    //!<Const accessor type to every value related to any element B, i.e., f(x,B).
        private:
                std::set<T> elements;   //!<Actual set of elements.
                MatrixType relation;    //!<Matrix storing the relation.

                /**
                  * Template used to make the function interface independent from the function type.
                  *  (wrapper for the global method - needed to make this compile under GCC).
                  */
                template<typename FunctionType> inline void applyFunction(FunctionType fun,size_t e1,size_t e2,const T &T1,const T &T2) {
                        detail::applyFunction<T,U,UIsObject,FunctionType>(*this,fun,e1,e2,T1,T2);
                }

        public:
                /**
                  * Default constructor, doesn't initialize the relation.
                  */
                explicit inline CBinaryRelation(const std::set<T> &els):elements(els),relation(els.size(),els.size())   {}
                /**
                  * Constructor which initializes the relation using a given function.
                  */
                template<typename FunctionType> inline CBinaryRelation(const std::set<T> &els,FunctionType fun):elements(els),relation(els.size(),els.size())   {
                        initializeWith(fun);
                }
                /**
                  * Initialize the whole relation with a given function.
                  */
                template<typename FunctionType> void initializeWith(FunctionType fun)   {
                        typename std::set<T>::const_iterator it=elements.begin();
                        for (size_t i=0;i<elements.size();++i,++it)     {
                                typename std::set<T>::const_iterator jt=elements.begin();
                                for (size_t j=0;j<elements.size();++j,++jt) applyFunction(fun,i,j,*it,*jt);
                        }
                }
                /**
                  * Initialize the whole relation with a given function, assuming that the relation is symmetrical.
                  */
                template<typename FunctionType> void initializeSymmetricallyWith(FunctionType fun)      {
                        typename std::set<T>::const_iterator it=elements.begin();
                        for (size_t i=0;i<elements.size();++i,++it)     {
                                applyFunction(fun,i,i,*it,*it);
                                typename std::set<T>::const_iterator jt=it;
                                jt++;
                                for (size_t j=i+1;j<elements.size();++j,++jt)   {
                                        applyFunction(fun,i,j,*it,*jt);
                                        relation(j,i)=relation(i,j);
                                }
                        }
                }
                /**
                  * Manually set a relationship value, given the indices.
                  */
                inline void setRelationValue(size_t e1,size_t e2,const U &newVal)       {
                        relation.get_unsafe(e1,e2)=newVal;
                }
                /**
                  * Get a relation value, given the indices.
                  */
                inline const U &getRelationValue(size_t e1,size_t e2) const     {
                        return relation.get_unsafe(e1,e2);
                }
                inline const U &operator()(size_t e1,size_t e2) const   {
                        return getRelationValue(e1,e2);
                }
                /**
                  * Get a reference to a relation value given its indices, which allows both querying and setting the value.
                  */
                inline U &getRelationValue(size_t e1,size_t e2) {
                        return relation.get_unsafe(e1,e2);
                }
                inline U &operator()(size_t e1,size_t e2)       {
                        return getRelationValue(e1,e2);
                }
                /**
                  * Manually set a relationship value, given the elements. Returns false if any of the elements is not present.
                  */
                inline bool setRelationValue(const T &t1,const T &t2,const U &newVal)   {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it1=std::find(b,e,t1),it2=std::find(b,e,t2);
                        if (it1==e||it2==e) return false;
                        setRelationValue(static_cast<size_t>(std::distance(b,it1)),static_cast<size_t>(std::distance(b,it2)),newVal);
                        return true;
                }
                /**
                  * Get a relation value, given the elements. Throws domain_error if any of the elements is not present.
                  */
                inline U getRelationValue(const T &t1,const T &t2) const        {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it1=std::find(b,e,t1),it2=std::find(b,e,t2);
                        if (it1==e||it2==e) throw std::domain_error("Element not found");
                        return getRelationValue(static_cast<size_t>(std::distance(b,it1)),static_cast<size_t>(std::distance(b,it2)));
                }
                /**
                  * Get a reference to a relation value given the elements, which allows both querying and setting. Throws domain_error if any of the elements is not
                  * present.
                  */
                inline U &getRelationValue(const T &t1,const T &t2)     {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it1=std::find(b,e,t1),it2=std::find(b,e,t2);
                        if (it1==e||it2==e) throw std::domain_error("Element not found");
                        return getRelationValue(static_cast<size_t>(std::distance(b,it1)),static_cast<size_t>(distance(b,it2)));
                }
                /**
                  * Gets an iterator to the starting point of the elements set.
                  */
                inline const_iterator begin() const     {
                        return elements.begin();
                }
                /**
                  * Gets an iterator to the ending point of the elements set.
                  */
                inline const_iterator end() const       {
                        return elements.end();
                }
                /**
                  * Gets a reverse iterator to the ending point of the elements set.
                  */
                inline const_reverse_iterator rbegin() const    {
                        return elements.rbegin();
                }
                /**
                  * Gets a reverse iterator to the starting point of the elements set.
                  */
                inline const_reverse_iterator rend() const      {
                        return elements.rend();
                }
                /**
                  * Operator for direct access to a element given its index.
                  */
                T operator[](size_t i) const    {
                        ASSERT_BELOW_(i,elements.size())
                        typename std::set<T>::const_iterator it=elements.begin();
                        std::advance(it,i);
                        return *it;
                }
                /**
                  * Gets an accessor for every value related to an element A given its index, i.e., every f(A,x). This accessor is iterable.
                  */
                inline AccessorForFirstElement getRelationFrom(size_t i)        {
                        return AccessorForFirstElement(relation,i);
                }
                /**
                  * Gets a constant accessor for every value related to an element A given its index, i.e., every f(A,x). This accessor is iterable.
                  */
                inline ConstAccessorForFirstElement getRelationFrom(size_t i) const     {
                        return ConstAccessorForFirstElement(relation,i);
                }
                /**
                  * Gets an accessor for every value related to an element B given its index, i.e., every f(x,B). This accessor is iterable.
                  */
                inline AccessorForSecondElement getRelationTo(size_t i) {
                        return AccessorForSecondElement(relation,i);
                }
                /**
                  * Gets a constant accessor for every value related to an element B given its index, i.e., every f(x,B). This accessor is fully iterable.
                  */
                inline ConstAccessorForSecondElement getRelationTo(size_t i) const      {
                        return ConstAccessorForSecondElement(relation,i);
                }
                /**
                  * Gets an iterable accessor for every value related to an element A, i.e., every f(A,x). A domain_error will be thrown if the element is not present.
                  */
                inline AccessorForFirstElement getRelationFrom(const T &t)      {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it=std::find(b,e,t);
                        if (it==e) throw std::domain_error("Element not found");
                        return getRelationFrom(static_cast<size_t>(std::distance(b,it)));
                }
                /**
                  * Gets an iterable constant accessor for every value related to an element A, i.e., every f(A,x). A domain_error will be thrown if the element is not
                  * present.
                  */
                inline ConstAccessorForFirstElement getRelationFrom(const T &t) const   {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it=std::find(b,e,t);
                        if (it==e) throw std::domain_error("Element not found");
                        return getRelationFrom(static_cast<size_t>(std::distance(b,it)));
                }
                inline void getRelationFrom(size_t i,vector<U> &vec)    {
                        size_t N=elements.size();
                        ASSERT_(i<N);
                        vec.resize(N);
                        ConstAccessorForFirstElement access(relation,i);
                        std::copy(access.begin(),access.end(),vec.begin());
                }
                inline void getRelationFrom(const T &t,vector<U> &vec)  {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it=std::find(b,e,t);
                        if (it==e) throw std::domain_error("Element not found");
                        getRelationFrom(static_cast<size_t>(std::distance(b,it)),vec);
                }
                /**
                  * Gets an iterable accessor for every value related to an element B, i.e., every f(x,B). A domain_error will be thrown if the element is not present.
                  */
                inline AccessorForSecondElement getRelationTo(const T &t)       {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it=std::find(b,e,t);
                        if (it==e) throw std::domain_error("Element not found");
                        return getRelationTo(static_cast<size_t>(std::distance(b,it)));
                }
                /**
                  * Gets an iterable constant accessor for every value related to an alement B, i.e., every f(x,B). A domain_error will be thrown if the element is not
                  * present.
                  */
                inline ConstAccessorForSecondElement getRelationTo(const T &t) const    {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it=std::find(b,e,t);
                        if (it==e) throw std::domain_error("Element not found");
                        return getRelationTo(static_cast<size_t>(std::distance(b,it)));
                }
                inline void getRelationTo(size_t i,vector<U> &vec)      {
                        size_t N=elements.size();
                        ASSERT_(i<N);
                        vec.resize(N);
                        ConstAccessorForSecondElement access(relation,i);
                        std::copy(access.begin(),access.end(),vec.begin());
                }
                inline void getRelationTo(const T &t,vector<U> &vec)    {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it=std::find(b,e,t);
                        if (it==e) throw std::domain_error("Element not found");
                        getRelationTo(static_cast<size_t>(std::distance(b,it)),vec);
                }
                /**
                  * Removes an element at a concrete position.
                  */
                void removeElementAt(size_t i)  {
                        ASSERT_(i<elements.size());
                        typename std::set<T>::const_iterator it=elements.begin();
                        std::advance(it,i);
                        elements.erase(i);
                        set<size_t> ii;
                        ii.insert(i);
                        relation.removeRowsAndCols(ii,ii);
                }
                /**
                  * Removes an element. Returns false if the element was not present and thus could'nt be eliminated.
                  */
                bool removeElement(const T &el) {
                        typename std::set<T>::const_iterator b=elements.begin(),e=elements.end();
                        typename std::set<T>::const_iterator it=std::find(e,b,el);
                        if (it==e) return false;
                        removeElementAt(std::distance(b,it));
                        return true;
                }
                /**
                  * Removes a set of elements. Returns the number of elements which were actually erased.
                  */
                size_t removeElements(const set<T> &vals)       {
                        set<size_t> positions;
                        for (typename set<T>::const_iterator it=vals.begin();it!=vals.end();++it)       {
                                typename set<T>::iterator elsIt=std::find(elements.begin(),elements.end(),*it);
                                if (elsIt!=elements.end()) positions.insert(std::distance(elements.begin(),elsIt));
                        }
/*                      for (set<size_t>::const_reverse_iterator it=positions.rbegin();it!=positions.rend();++it)       {
                                typename std::set<T>::const_iterator it2=elements.begin();
                                std::advance(it2,*it);
                                elements.erase(it2);
                        }
                        relation.removeRowsAndCols(positions,positions);*/
                        removeElementsAt(positions);
                        return positions.size();
                }
                void removeElementsAt(const set<size_t> &poss)  {
                        relation.removeRowsAndCols(poss,poss);
                        for (set<size_t>::const_reverse_iterator it=poss.rbegin();it!=poss.rend();++it) {
                                typename std::set<T>::const_iterator it2=elements.begin();
                                std::advance(it2,*it);
                                elements.erase(it2);
                        }
                }
                /**
                  * Inserts an element. If the element was present, returns false and its current position. If it wasn't, returns true and the position in which it was
                  * inserted.
                  */
                std::pair<bool,size_t> insertElement(const T &el)       {
                        std::pair<typename std::set<T>::iterator,bool> ins=elements.insert(el);
                        size_t dist=std::distance(elements.begin(),ins.first);
                        if (ins.second) {
                                std::multiset<size_t> newEls;
                                newEls.insert(dist);
                                relation.insertRowsAndCols(newEls,newEls);
                                return std::make_pair(true,dist);
                        }       else return std::make_pair(false,dist);
                }
                /**
                  * Inserts an element and initializes its relationship values, even if it was already present.
                  */
                template<typename FunctionType> std::pair<bool,size_t> insertElement(const T &el,FunctionType fun)      {
                        std::pair<bool,size_t> ins=insertElement(el);
                        size_t pos=ins.second;
                        for (size_t i=0;i<elements.size();++i)  {
                                const T &newEl=operator[](i);
                                applyFunction(fun,pos,i,el,newEl);
                                applyFunction(fun,i,pos,newEl,el);
                        }
                        return ins;
                }
                /**
                  * Inserts a set of elements into the relation. Does not initialize the actual relation.
                  */
                size_t insertElements(const std::set<T> &els)   {
                        if (els.empty()) return 0;
                        //This code is much more complex than it should! Trying, for efficiency, to avoid multiple calls to insertElement makes things a lot harder.
                        //It raises the complexity level to N², but alleviates it greatly by making a single memory allocation. Multiple calls to insertElement will be
                        //faster only if the number of elements in the set is really large.
                        std::vector<size_t> added;
                        //std::vector<size_t> exist;
                        added.reserve(els.size());
                        for (typename std::set<T>::const_iterator it=els.begin();it!=els.end();++it)    {
                                std::pair<typename std::set<T>::iterator,bool> ins=elements.insert(*it);
                                size_t dist=std::distance(elements.begin(),ins.first);
                                if (ins.second) {
                                        added.push_back(dist);
                                        for (std::vector<size_t>::iterator it2=added.begin();it2!=added.end();++it2) if (*it2>=dist) ++(*it2);
                                        //for (std::vector<size_t>::iterator it2=exist.begin();it2!=exist.end();++it2) if (*it2>=dist) ++(*it2);
                                }//     else exist.push_back(dist);
                        }
                        std::sort(added.begin(),added.end());
                        for (size_t j=1;j<added.size();++j) added[j]-=j;
                        std::multiset<size_t> poss(added.begin(),added.end());
                        relation.insertRowsAndCols(poss,poss);
                        return added.size();
                }
                /**
                  * Inserts a set of elements into the relation, initializing the actual relation with a given function.
                  */
                template<typename FunctionType> size_t insertElements(const std::set<T> &els,FunctionType fun)  {
                        if (els.empty()) return 0;
                        size_t howMany=insertElements(els);
                        std::set<size_t> poss;
                        {
                                //Little scope for "begin" and "end"...
                                typename std::set<T>::const_iterator begin=elements.begin(),end=elements.end();
                                for (typename std::set<T>::const_iterator it=els.begin();it!=els.end();++it) poss.insert(std::distance(begin,find(begin,end,*it)));
                        }
                        std::set<size_t> nPoss;
                        std::set<size_t>::const_iterator begin=poss.begin(),end=poss.end();
                        for (size_t i=0;i<elements.size();++i) if (std::find(begin,end,i)==end) nPoss.insert(i);
                        vector<const T *> proxy;
                        proxy.reserve(poss.size());
                        for (std::set<size_t>::const_iterator it=begin;it!=end;++it)    {
                                const T &e1=operator[](*it);
                                proxy.push_back(&e1);
                                size_t i=0;
                                for (typename std::set<T>::const_iterator it2=elements.begin();it2!=elements.end();++it2,++i) applyFunction(fun,*it,i,e1,*it2);
                        }
                        for (std::set<size_t>::const_iterator it=nPoss.begin();it!=nPoss.end();++it)    {
                                const T &e1=operator[](*it);
                                typename std::vector<const T *>::const_iterator itV=proxy.begin();
                                for (std::set<size_t>::const_iterator it2=poss.begin();it2!=poss.end();++it2,++itV) applyFunction(fun,*it,*it2,e1,**itV);
                        }
                        return howMany;
                }
                /**
                  * Completely resets the relation, using a new set of elements. Does not initialize the relation.
                  */
                void setElements(const std::set<T> &newEls)     {
                        relation.setSize(0,0);
                        elements=newEls;
                        relation.setSize(newEls.size(),newEls.size());
                }
                /**
                  * Returns the amount of elements present in the relation.
                  */
                inline size_t size() const      {
                        return elements.size();
                }
        };

        namespace detail {
                // generic version (specialization is after definition of CBinaryRelation):
                template<typename T,typename U,bool UIsObject,typename FunctionType> inline void applyFunction(CBinaryRelation<T,U,UIsObject> &o, FunctionType fun,size_t e1,size_t e2,const T &T1,const T &T2) {
                        o.getRelationValue(e1,e2)=fun(T1,T2);
                }

                /** Template specialization by reference type.
                  */
                template<typename T,typename U,bool UIsObject> inline void applyFunction(CBinaryRelation<T,U,UIsObject> &o,typename CBinaryRelation<T,U,UIsObject>::FunctionByReferencePass fun,size_t e1,size_t e2,const T &T1,const T &T2)    {
                        fun(T1,T2,o.getRelationValue(e1,e2));
                }
        }


}}      //End of namespaces
#endif